jepler/Adafruit_Learning_System_Guides
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Merge pull request #339 from dastels/master

Browse source 
Add WobblyBot code
This commit is contained in:
Limor "Ladyada" Fried 2018-09-24 16:30:52 -07:00 committed by GitHub
commit ec17ec0e95
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 745 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

57
Crickit_WobblyBot/WobblyBot/BluefruitConfig.h Normal file
View file

@ -0,0 +1,57 @@
// COMMON SETTINGS
// ----------------------------------------------------------------------------------------------
// These settings are used in both SW UART, HW UART and SPI mode
// ----------------------------------------------------------------------------------------------
#define BUFSIZE 128 // Size of the read buffer for incoming data
#define VERBOSE_MODE true // If set to 'true' enables debug output
#define BLE_READPACKET_TIMEOUT 500 // Timeout in ms waiting to read a response
// SOFTWARE UART SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the pins that will be used for 'SW' serial.
// You should use this option if you are connecting the UART Friend to an UNO
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_SWUART_RXD_PIN 9 // Required for software serial!
#define BLUEFRUIT_SWUART_TXD_PIN 10 // Required for software serial!
#define BLUEFRUIT_UART_CTS_PIN 11 // Required for software serial!
#define BLUEFRUIT_UART_RTS_PIN -1 // Optional, set to -1 if unused
// HARDWARE UART SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the HW serial port you are using. Uncomment
// this line if you are connecting the BLE to Leonardo/Micro or Flora
// ----------------------------------------------------------------------------------------------
#ifdef Serial1 // this makes it not complain on compilation if there's no Serial1
#define BLUEFRUIT_HWSERIAL_NAME Serial1
#endif
// SHARED UART SETTINGS
// ----------------------------------------------------------------------------------------------
// The following sets the optional Mode pin, its recommended but not required
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_UART_MODE_PIN 12 // Set to -1 if unused
// SHARED SPI SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the pins to use for HW and SW SPI communication.
// SCK, MISO and MOSI should be connected to the HW SPI pins on the Uno when
// using HW SPI. This should be used with nRF51822 based Bluefruit LE modules
// that use SPI (Bluefruit LE SPI Friend).
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_SPI_CS 8
#define BLUEFRUIT_SPI_IRQ 7
#define BLUEFRUIT_SPI_RST 4 // Optional but recommended, set to -1 if unused
// SOFTWARE SPI SETTINGS
// ----------------------------------------------------------------------------------------------
// The following macros declare the pins to use for SW SPI communication.
// This should be used with nRF51822 based Bluefruit LE modules that use SPI
// (Bluefruit LE SPI Friend).
// ----------------------------------------------------------------------------------------------
#define BLUEFRUIT_SPI_SCK 13
#define BLUEFRUIT_SPI_MISO 12
#define BLUEFRUIT_SPI_MOSI 11

411
Crickit_WobblyBot/WobblyBot/WobblyBot.ino Normal file
View file

@ -0,0 +1,411 @@
// Continuous servo based walking/waddling/etc robot.
// Bluetooth code is from Feather M0 Bluefruit controller example.
// Explainatory comments kept intact.
// Adafruit invests time and resources providing this open source code.
// Please support Adafruit and open source hardware by purchasing
// products from Adafruit!
// Written by Dave Astels for Adafruit Industries
// Copyright (c) 2018 Adafruit Industries
// Licensed under the MIT license.
// All text above must be included in any redistribution.
#include <stdarg.h>
#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"
#include "BluefruitConfig.h"
#include "Adafruit_Crickit.h"
#include "seesaw_servo.h"
#include "seesaw_motor.h"
#define FACTORYRESET_ENABLE 1
#define MINIMUM_FIRMWARE_VERSION "0.6.6"
#define MODE_LED_BEHAVIOUR "MODE"
// function prototypes over in packetparser.cpp
uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout);
float parsefloat(uint8_t *buffer);
void printHex(const uint8_t * data, const uint32_t numBytes);
// the packet buffer
extern uint8_t packetbuffer[];
//#define DEBUG 1
Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);
//------------------------------------------------------------------------------
// setup crickit
Adafruit_Crickit crickit;
seesaw_Servo legs[] = {seesaw_Servo(&crickit),
seesaw_Servo(&crickit),
seesaw_Servo(&crickit),
seesaw_Servo(&crickit)};
const int front_right = 0;
const int front_left = 1;
const int rear_right = 2;
const int rear_left = 3;
seesaw_Motor tail(&crickit);
float tail_power = 0.5;
//------------------------------------------------------------------------------
// conditional output routines
void error(const __FlashStringHelper *err)
{
digitalWrite(13, HIGH);
#ifdef DEBUG
Serial.println(err);
#endif
while (1);
}
void log(const __FlashStringHelper *msg)
{
#ifdef DEBUG
Serial.println(msg);
#endif
}
//------------------------------------------------------------------------------
// Motor Control
// Left and right motors turn in the opposite direction
const float motor_directions[4] = {+1.0, -1.0, +1.0, -1.0};
// pins to connect each servo to
const int servo_pins[4] = {CRICKIT_SERVO1, CRICKIT_SERVO2, CRICKIT_SERVO3, CRICKIT_SERVO4};
// PWM ranges for each motor, tune these so that setting the angle to 90 stops the motor
int pwm_ranges[4][2] = {{500, 2400}, {500, 2400}, {500, 2400}, {500, 2400}};
const __FlashStringHelper *leg_names[] = {F("Front right"), F("Front left"), F("Rear right"), F("Rear left")};
int speed_to_angle(float speed)
{
return (int)(speed * 90.0 + 90.0);
}
void set_leg(int leg, float speed)
{
int angle = speed_to_angle(speed * motor_directions[leg]);
#ifdef DEBUG
Serial.print(F("Setting "));
Serial.print(leg_names[leg]);
Serial.print(F(" to "));
Serial.println(angle);
#endif
legs[leg].write(angle);
}
// Stop the listed motors
// -1 required as the last argument
void stop(int leg, ...)
{
va_list args;
va_start(args, leg);
log(F("Stop"));
while (leg != -1) {
set_leg(leg, 0.0);
leg = va_arg(args, int);
}
va_end(args);
}
void stop_all()
{
stop(front_right, front_left, rear_right, rear_left, -1);
}
void forward(float speed, ...)
{
va_list args;
va_start(args, speed);
int leg = va_arg(args, int);
log(F("Forward"));
while (leg != -1) {
set_leg(leg, speed * motor_directions[leg]);
leg = va_arg(args, int);
}
va_end(args);
}
void forward_all(float speed)
{
forward(speed, front_right, front_left, rear_right, rear_left, -1);
}
void reverse(float speed, ...)
{
va_list args;
va_start(args, speed);
int leg = va_arg(args, int);
log(F("Reverse"));
while (leg != -1) {
set_leg(leg, speed * -1 * motor_directions[leg]);
leg = va_arg(args, int);
}
va_end(args);
}
void reverse_all(float speed)
{
reverse(speed, front_right, front_left, rear_right, rear_left, -1);
}
void rotate_clockwise(float speed)
{
forward(speed, front_left, rear_left, -1);
reverse(speed, front_right, rear_right, -1);
}
void rotate_counterclockwise(float speed)
{
forward(speed, front_right, rear_right, -1);
reverse(speed, front_left, rear_left, -1);
}
void initialize()
{
stop(front_right, front_left, rear_right, rear_left, -1);
}
void wag(float speed)
{
#ifdef DEBUG
Serial.print(F("Wag "));
Serial.println(speed);
#endif
tail.throttle(speed);
delay(75);
tail.throttle(0.0);
delay(50);
}
//------------------------------------------------------------------------------
// Start things up
void setup()
{
pinMode(13, OUTPUT);
digitalWrite(13, LOW);
#ifdef DEBUG
while (!Serial); // required for Flora & Micro
delay(500);
Serial.begin(115200);
#endif
log(F("WobblyBot"));
log(F("-----------------------------------------"));
// Initialise the module
log(F("Initialising the Bluefruit LE module: "));
if ( !ble.begin(VERBOSE_MODE) )
{
error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
}
log( F("OK!") );
if ( FACTORYRESET_ENABLE )
{
// Perform a factory reset to make sure everything is in a known state
log(F("Performing a factory reset: "));
if ( ! ble.factoryReset() ){
error(F("Couldn't factory reset"));
}
}
// Disable command echo from Bluefruit
ble.echo(false);
log(F("Requesting Bluefruit info:"));
// Print Bluefruit information
ble.info();
log(F("Please use Adafruit Bluefruit LE app to connect in Controller mode"));
log(F("Then activate/use the sensors, color picker, game controller, etc!\n"));
ble.verbose(false); // debug info is a little annoying after this point!
// Wait for connection
while (! ble.isConnected()) {
delay(500);
}
log(F("******************************"));
// LED Activity command is only supported from 0.6.6
if ( ble.isVersionAtLeast(MINIMUM_FIRMWARE_VERSION) )
{
// Change Mode LED Activity
log(F("Change LED activity to " MODE_LED_BEHAVIOUR));
ble.sendCommandCheckOK("AT+HWModeLED=" MODE_LED_BEHAVIOUR);
}
// Set Bluefruit to DATA mode
log( F("Switching to DATA mode!") );
ble.setMode(BLUEFRUIT_MODE_DATA);
log(F("******************************"));
if (!crickit.begin()) {
error(F("Error initializing CRICKIT!"));
}
log(F("Crickit started"));
for (int leg = 0; leg < 4; leg++) {
legs[leg].attach(servo_pins[leg], pwm_ranges[leg][0], pwm_ranges[leg][1]);
}
tail.attach(CRICKIT_MOTOR_A1, CRICKIT_MOTOR_A2);
}
// Fill these functions in with the movement scripts you want attached to
// the controller's 1-4 buttons
void demo1()
{
forward_all(0.5);
delay(5000);
rotate_clockwise(0.5);
delay(2000);
forward_all(0.75);
delay(4000);
rotate_counterclockwise(0.5);
delay(3000);
stop_all();
}
void demo2()
{
}
void demo3()
{
}
void demo4()
{
}
//------------------------------------------------------------------------------
// Main loop
void loop()
{
wag(tail_power);
tail_power *= -1.0;
// Wait for new data to arrive
uint8_t len = readPacket(&ble, BLE_READPACKET_TIMEOUT);
if (len == 0) return;
// Got a packet!
// printHex(packetbuffer, len);
// Buttons
if (packetbuffer[1] == 'B') {
uint8_t buttnum = packetbuffer[2] - '0';
boolean pressed = packetbuffer[3] - '0';
#ifdef DEBUG
Serial.print ("Button "); Serial.print(buttnum);
if (pressed) {
Serial.println(" pressed");
} else {
Serial.println(" released");
}
#endif
switch(buttnum) {
case 1:
if (pressed) {
demo1();
}
break;
case 2:
if (pressed) {
demo2();
}
break;
case 3:
if (pressed) {
demo3();
}
break;
case 4:
if (pressed) {
demo4();
}
break;
case 5:
if (pressed) {
rotate_counterclockwise(0.5);
} else {
stop_all();
}
break;
case 6:
if (pressed) {
rotate_clockwise(0.5);
} else {
stop_all();
}
break;
case 7:
if (pressed) {
reverse_all(0.5);
} else {
stop_all();
}
break;
case 8:
if (pressed) {
forward_all(0.5);
} else {
stop_all();
}
break;
}
}
}

141
Crickit_WobblyBot/WobblyBot/packetParser.cpp Normal file
View file

@ -0,0 +1,141 @@
#include <string.h>
#include <Arduino.h>
#include <SPI.h>
#if not defined (_VARIANT_ARDUINO_DUE_X_) && not defined (_VARIANT_ARDUINO_ZERO_) && not defined(__SAMD51__)
#include <SoftwareSerial.h>
#endif
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"
#define PACKET_ACC_LEN (15)
#define PACKET_GYRO_LEN (15)
#define PACKET_MAG_LEN (15)
#define PACKET_QUAT_LEN (19)
#define PACKET_BUTTON_LEN (5)
#define PACKET_COLOR_LEN (6)
#define PACKET_LOCATION_LEN (15)
// READ_BUFSIZE Size of the read buffer for incoming packets
#define READ_BUFSIZE (20)
/* Buffer to hold incoming characters */
uint8_t packetbuffer[READ_BUFSIZE+1];
/**************************************************************************/
/*!
@brief Casts the four bytes at the specified address to a float
*/
/**************************************************************************/
float parsefloat(uint8_t *buffer)
{
float f;
memcpy(&f, buffer, 4);
return f;
}
/**************************************************************************/
/*!
@brief Prints a hexadecimal value in plain characters
@param data Pointer to the byte data
@param numBytes Data length in bytes
*/
/**************************************************************************/
void printHex(const uint8_t * data, const uint32_t numBytes)
{
uint32_t szPos;
for (szPos=0; szPos < numBytes; szPos++)
{
Serial.print(F("0x"));
// Append leading 0 for small values
if (data[szPos] <= 0xF)
{
Serial.print(F("0"));
Serial.print(data[szPos] & 0xf, HEX);
}
else
{
Serial.print(data[szPos] & 0xff, HEX);
}
// Add a trailing space if appropriate
if ((numBytes > 1) && (szPos != numBytes - 1))
{
Serial.print(F(" "));
}
}
Serial.println();
}
/**************************************************************************/
/*!
@brief Waits for incoming data and parses it
*/
/**************************************************************************/
uint8_t readPacket(Adafruit_BLE *ble, uint16_t timeout)
{
uint16_t origtimeout = timeout, replyidx = 0;
memset(packetbuffer, 0, READ_BUFSIZE);
while (timeout--) {
if (replyidx >= 20) break;
if ((packetbuffer[1] == 'A') && (replyidx == PACKET_ACC_LEN))
break;
if ((packetbuffer[1] == 'G') && (replyidx == PACKET_GYRO_LEN))
break;
if ((packetbuffer[1] == 'M') && (replyidx == PACKET_MAG_LEN))
break;
if ((packetbuffer[1] == 'Q') && (replyidx == PACKET_QUAT_LEN))
break;
if ((packetbuffer[1] == 'B') && (replyidx == PACKET_BUTTON_LEN))
break;
if ((packetbuffer[1] == 'C') && (replyidx == PACKET_COLOR_LEN))
break;
if ((packetbuffer[1] == 'L') && (replyidx == PACKET_LOCATION_LEN))
break;
while (ble->available()) {
char c = ble->read();
if (c == '!') {
replyidx = 0;
}
packetbuffer[replyidx] = c;
replyidx++;
timeout = origtimeout;
}
if (timeout == 0) break;
delay(1);
}
packetbuffer[replyidx] = 0; // null term
if (!replyidx) // no data or timeout
return 0;
if (packetbuffer[0] != '!') // doesn't start with '!' packet beginning
return 0;
// check checksum!
uint8_t xsum = 0;
uint8_t checksum = packetbuffer[replyidx-1];
for (uint8_t i=0; i<replyidx-1; i++) {
xsum += packetbuffer[i];
}
xsum = ~xsum;
// Throw an error message if the checksum's don't match
if (xsum != checksum)
{
Serial.print("Checksum mismatch in packet : ");
printHex(packetbuffer, replyidx+1);
return 0;
}
// checksum passed!
return replyidx;
}

136
Crickit_WobblyBot/code.py Normal file
View file

@ -0,0 +1,136 @@
"""
Continuous servo based walking/waddling/etc robot.
Adafruit invests time and resources providing this open source code.
Please support Adafruit and open source hardware by purchasing
products from Adafruit!
Written by Dave Astels for Adafruit Industries
Copyright (c) 2018 Adafruit Industries
Licensed under the MIT license.
All text above must be included in any redistribution.
"""
import time
from adafruit_crickit import crickit
tail = crickit.dc_motor_1
# Each servo corresponds to one of the legs
front_right = crickit.continuous_servo_1
front_left = crickit.continuous_servo_2
rear_right = crickit.continuous_servo_3
rear_left = crickit.continuous_servo_4
# Useful groups of legs
all_legs = [front_right, front_left, rear_right, rear_left]
front_legs = [front_right, front_left]
rear_legs = [rear_right, rear_left]
right_legs = [front_right, rear_right]
left_legs = [front_left, rear_left]
# The sign (+1/-1) for forward motion for each servo
direction_values = {front_right: +1, front_left: -1, rear_right: +1, rear_left: -1}
# Tweak the pwn ranges for each servo so that throttle of 0 stops the motor
pwm_ranges = {front_right: (500, 2400), front_left: (500, 2400), rear_right: (500, 2400), rear_left: (500, 2400)}
def init():
for leg in all_legs:
limits = pwm_ranges[leg]
leg.set_pulse_width_range(min_pulse=limits[0], max_pulse=limits[1])
leg.throttle = 0
def wag(speed):
tail.throttle = speed
time.sleep(0.1)
tail.throttle = 0.0
time.sleep(0.25)
def wag_for(seconds):
target_time = time.monotonic() + seconds
wag_throttle = 1.0
while time.monotonic() < target_time:
wag(wag_throttle)
wag_throttle *= -1
def forward(servo_or_servos, speed):
if type(servo_or_servos) == list:
for servo in servo_or_servos:
servo.throttle = speed * direction_values[servo]
else:
servo_or_servos.throttle = speed * direction_values[servo_or_servos]
def reverse(servo_or_servos, speed):
if type(servo_or_servos) == list:
for servo in servo_or_servos:
servo.throttle = speed * -1 * direction_values[servo]
else:
servo_or_servos.throttle = speed * -1 * direction_values[servo_or_servos]
def stop(servo_or_servos):
if type(servo_or_servos) == list:
for servo in servo_or_servos:
servo.throttle = 0
else:
servo_or_servos.throttle = 0
def rotate_clockwise(speed):
forward(left_legs, speed)
reverse(right_legs, speed)
def rotate_counterclockwise(speed):
forward(right_legs, speed)
reverse(left_legs, speed)
def crawl_forward(speed):
forward(all_legs, speed)
def crawl_backward(speed):
reverse(all_legs, speed)
def turtle():
stop([rear_right, rear_left])
stop(rear_left)
forward(front_right, 0.5)
forward(front_left, 0.5)
def snake_step():
stop(all_legs)
forward(right_legs, 0.5)
time.sleep(1.0)
stop(right_legs)
forward(left_legs, 0.5)
time.sleep(1.0)
stop(left_legs)
init()
def demo1():
crawl_forward(0.5)
wag_for(5.0)
rotate_clockwise(0.25)
wag_for(3.0)
crawl_backward(0.5)
wag_for(2.0)
rotate_counterclockwise(0.25)
wag_for(3.0)
crawl_forward(0.5)
wag_for(5.0)
stop(all_legs)
demo1()