This build will use the FeatherWing CRICKIT. The Feather you choose will determine your language options as well as any non-CRICKIT capabilities. Here we'll use the Feather M0 Bluefruit and write the code in C/C++ using the Arduino framework and tools. This board was selected for it's bluetooth capabilities which will let us drive the robot from the Adafruit Bluefruit app controller interface, as well as run pre-written sequences using the controller's 1-4 buttons.
You'll need to have the Arduino IDE installed as well as the appropriate board packages and libraries. The Feather M0 Bluefruit LE guide covers this in detail. It's a good idea to read through that guide if you haven't yet; it will show you all the tricks of this board.
This code has a couple support files with it that were copied from the controller.ino example: BluefruitConfig.h and packetParser.cpp. Either start a new sketch called WobblyBot and copy them into the directory along with the WobblyBot.ino file below. Or clone the repo from github; it has everything in place that's needed. In either case, load WobblyBot into the Arduino IDE, set your board and port (see the linked guide for this Feather board), and compile/upload the code.
Use of this version of the CRICKIT is identical to the others. Connect the servos to the appropriate connectors on the CRICKIT, and connect the tail motor to the motor 1 connections. The smaller LiPo powers the Feather board. Using a Feather has the advantage that the battery doesn't need to be disconnected to charge it. It will be charging whenever the Feather is connected via USB. Finally connect the 5v supply to the CRICKIT.
We won't go into detail on the boilerplate BLE setup and use. That's covered in the linked guide and the comments from the example code that was used have been kept intact.
While the structure of the C++ code is similar to the Python, the details are, naturally, quite different.
We start by creating servo objects, placing them in an array. We define a constant index for each leg. We have the multipliers to correct for servo orientation, which servo pin to use for each leg, and the limit settings to tweak. Additionally there is an array of strings used in debug output for leg names.
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;
// 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")};
Speaking of debugging, there are two small functions for outputting errors and information. If you want to output to the serial console simple uncomment the #define DEBUG line near the start of the file. Remember to recompile/upload with it commented out before running the robot untethered.
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
}
Using the seesaw_servo library lets us set servo angles, but there is no direct support for setting speed as in the Python library, we need to set the raw angle. To abstract this away, there's the function speed_to_angle. It takes a floating point speed from -1.0 to 1.0 and converts it to an integer angle from 0 to 180.
To hide the details, and to provide a single place to log servo settings, we have the set_leg function.
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);
}
Now we have the forward/reverse/stop functions. This is quite different than Python. In the Python version we could pass a single leg or a list of legs and the function looked at what it was given and did the right thing. C++ does not have that capability due to it's very different approach to typing. It does, however, have a way to send a variable number of arguments to a function. We'll use that here. As an example, consider the stop function:
// 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);
}
We are using the variable argument support provided by the va_* macros/functions. This starts in the function's signature: the ... in the parameter list tells the compiler that this function can have an arbitrary number of arguments (but at least one: leg). the first thing the function does is create a variable of type va_list. This will hold the bookkeeping information for accessing the arguments. That variable gets initialized using va_start, passing it the va_list and the name of the argument immediately before the ...in the parameter list.
As it traverses through the arguments, note that it doesn't have names to use to get the values of any past the first one. Instead va_arg is used. This is passed the va_list that was initialized earlier as well as the type of the next expected argument. In our case they are all leg indexes so are all int.
Notice the comment saying that the last argument has to be -1. -1 is not a valid leg index (they are 0-4) so we are using that to mark the end of the arguments. The while loop continues to set leg speeds to 0 and fetching the next leg index until a -1 is found. This is called a sentinel value. Its use is solely to be the final argument so the loop knows when to stop.
The stop_all function shows a use of this.
The forward and reverse functions work in the same way, except that a speed value is the initial argument.
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);
}
The rotation functions use the forward and reverse functions as well, this time with 2 legs each.
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);
}
The tail works the same way as in the Python code. The only real difference is that the wagging is handled in the main loop rather than in a timed loop because we want to check for commands frequently.
seesaw_Motor tail(&crickit);
boolean tail_power = 0.5;
//...
void wag(float speed)
{
tail.throttle(speed);
delay(75);
tail.throttle(0.0);
delay(50);
}
//...
void loop()
{
wag(tail_power);
tail_power *= -1.0;
//...
}
The rest of the main loop checks for a command via BLE and performs the appropriate action based on which button was pressed (or released).
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;
}
}
}
In case you haven't run into the switch statement before, it chooses a case block based on the value given to it, buttnum in this case. The case block with the corresponding value is executed. Notice that each case block ends with a break statement. This exits the switch. If they were't there, the next case block would be executed, and so on until the end of the switch was reached or a break statement was encountered.
The entire WobblyBot.ino is below.
// SPDX-FileCopyrightText: 2018 Dave Astels for Adafruit Industries
//
// SPDX-License-Identifier: MIT
// 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;
}
}
}
Text editor powered by tinymce.
