diff --git a/Crickit_WobblyBot/WobblyBot/BluefruitConfig.h b/Crickit_WobblyBot/WobblyBot/BluefruitConfig.h new file mode 100644 index 00000000..86260e3f --- /dev/null +++ b/Crickit_WobblyBot/WobblyBot/BluefruitConfig.h @@ -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 diff --git a/Crickit_WobblyBot/WobblyBot/WobblyBot.ino b/Crickit_WobblyBot/WobblyBot/WobblyBot.ino new file mode 100644 index 00000000..7e7f85ad --- /dev/null +++ b/Crickit_WobblyBot/WobblyBot/WobblyBot.ino @@ -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 +#include +#include +#include +#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; + } + } +} diff --git a/Crickit_WobblyBot/WobblyBot/packetParser.cpp b/Crickit_WobblyBot/WobblyBot/packetParser.cpp new file mode 100644 index 00000000..29942be5 --- /dev/null +++ b/Crickit_WobblyBot/WobblyBot/packetParser.cpp @@ -0,0 +1,141 @@ +#include +#include +#include +#if not defined (_VARIANT_ARDUINO_DUE_X_) && not defined (_VARIANT_ARDUINO_ZERO_) && not defined(__SAMD51__) + #include +#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