Has anyone added a joystick control to the Balboa. I’m thinking about buying a bunch of these for a STEM event and will need to do more than just making it balance, Help!
Drew
Hello, Drew.
I’ve done that with an RC transmitter, but the code is kind of messy, so I’m not sure how useful it would be for you. Do you have modular RC transmitters and receivers or is there some other joystick and wireless transmitting control system you had in mind?
-Nathan
Nathan,
I didn’t have anything in particular in mind other than being able to make
it do something more than just balance. I was thinking it would be nice to
be able to drive it around with the keyboard or a joystick connection to a
PC using wireless. That’s all. Do you know if anyone has done this?
Drew
Unfortunately, I don’t know of any specific projects like this, but it sounds generally doable. I suggest looking for a wireless serial module that you can use to send TTL serial commands to the Balboa (such as our Wixels). You could then program the Balboa to respond to some basic serial command protocol. The programming on the Balboa side might be relatively straightforward as the “Balancer” example code in our library contains a high-level “balanceDrive()” function that can be used to control the average left and right motor speeds while the robot balances.
-Nathan
Nathan,
Thanks. If this is Arduino based then i should be able to do it without
issue. I’m putting in my order tonight.
Drew
I added the BLE board from Adafruit and can control it from my phone. Here’s the video:
If you’re interested, i can post the code.
–Tim
Tim,
This is fantastic, did you by chance add the details somewhere on how you did it?
Drew
I haven’t until now. The code and library are up on Github along with some images of the wiring. One thing to note is that I had to tweak the SoftwareSerial library to swap out an interrupt so the serial port would not conflict (see line 229). The BLE board requires serial communication. It would not compile until I made the tweak.
Have fun!
–Tim
NathanB,
Wow Cool We’re working with HS students. Do you thin it would be too messy for them?
Drew
Wow, that is too cool I just watched the video. the students are going to love this.
Tim,
Can you tell me what you used on the phone to make that happen?
Drew
Thank you!
Sure, the Adafruit BLE board uses an app available in the apple appstore or from google play. IOS or Android
There is some soldering, small parts, and crimping involved. You might want to build one for yourself first and see how it goes
I am not sure if it is too messy. Here is the code, though it will almost certainly require some modification for your application (the PID constants will probably need to be modified). You might be able to use the code for reading the RC receiver values with our other balancing examples (which are explained in this series of blog posts). The balancing algorithm in my code is a little bit different from (and probably not as robust as) the algorithm described in those blog posts.
-Nathan
RC_Balancer.ino (12.6 KB)
#include <Wire.h>
#include <Balboa32U4.h>
#include <LSM6.h>
LSM6 imu;
Balboa32U4Encoders encoders;
Balboa32U4Buzzer buzzer;
Balboa32U4Motors motors;
Balboa32U4ButtonA buttonA;
Balboa32U4ButtonB buttonB;
Balboa32U4ButtonC buttonC;
Balboa32U4LCD lcd;
//IMU internal sampling rates are 1.66kHz (1 sample every 0.60ms)
//Reading data takes about 1.9ms
//Default gyro is 245dps (-32766 -> +32766)
//Default accel is +- 2g (-32764 -> +32749)
//RC transmitter pulses at 60Hz (1 sample every 16ms)
//Reading data takes about 36ms
//ISRs for these take about 8-16us
//Encoder reads are very quick (about 4-16us).
//12 ticks/Motor rpm
//50 Motor rpm/output rpm
//220mm / output rpm
//1 tick = 0.366mm
int16_t countsLeft = 0; //Left encoder count
int16_t countsRight = 0; //Right encoder count
int16_t targetEncoderCount = 0; //Encoder count to target
double errorEncoderCount = 0; //Error in the encoder count
int16_t countsLeftLast = 0; //Left encoder count on last measurement
int16_t countsRightLast = 0; //Right encoder count on last measurement
uint32_t countsLastTS = 0; //Timestamp of the last encoder count measurement
double countRateLeft = 0; //Right channel encoder tick rate (ticks per millisecond)
double countRateRight = 0; //Right channel encoder tick rate (ticks per millisecond)
int16_t targetEncoderRate = 0; //Encoder change rate to target
double errorEncoderRate = 0; //Error in the encoder rate
//RC Control Variables
uint8_t xAxisRCPin = 1; //Arduino pin number of the xAxis RC signal (rotation) Channel 1
uint8_t yAxisRCPin = 0; //Arduino pin number of the yAxis RC signal (translation) Channel 2
int16_t xAxisPulseWidth = 1500; //Pulse width of the RC signal for X-axis rotation
int16_t yAxisPulseWidth = 1500; //Pulse width of the RC signal for Y-axis translation
uint32_t lastXrcTimeStamp; //microsecond timestamp of the last X axis RC pin change
uint32_t lastYrcTimeStamp; //microsecond timestamp of the last Y axis RC pin change
bool lastXRCState = LOW;
bool lastYRCState = LOW;
//Sensor Variables
int16_t yGyro; //y axis gyro reading
uint32_t lastImuTimeStamp; //microsecond timestamp
uint32_t thisImuTimeStamp; //microsecond timestamp
int16_t xAccel; //x axis accelerometer
int16_t zAccel; //z axis accelerometer
double xzAngle; //angle of acceleration between X and Z axes (degrees, 180 is up)
double filteredAngle; //angle from the complimentary filter
double balanceAngle = 172.2; //balance angle of the robot (degrees)
double targetAngle = balanceAngle; //target angle to balance at (degrees)
double errorAngle = 0; //Angle error (degrees)
double Ki = 0.11;
double Kp = 7.0;
double Kd = -1.10;
double iDecay = .95;
uint8_t activeK = 1; //K variable that is currently being adjusted
/*
const double Ki = 0.050;
const double Kp = 5.5;
const double Kd = -0.8;
const double iDecay = 0.90;
*/
double gyroRotationRate; //rotation rate after correction from offset (degrees per second)
double gyroZeroRotationRate; //rotation rate offset to apply (degrees per second)
double targetRotationRate = 0; //rotation rate to target (degrees per second)
double errorRotationRate = 0; //error in the roation rate
uint32_t imuNextSample = 0; //timestamp to take next gyro sample at (microseconds)
const uint32_t imuNextSampleInterval = 5000; //gyro sampling interval (microseconds)
double angleIntegral = 0; //(degrees * milliseconds)
double angleIntegralTarget = 0;
uint32_t pidNextAdjust = 0; //timestamp to run PID calculations and adjust the motor speed (microseconds)
const uint32_t pidNextAdjustInterval = 10000; //interval for control speed adjustments (microseconds)
double speedDifference;
int32_t motorSpeed;
double LRSpeedDifference = 0; //Speed different for Left and Right motor speeds
const uint16_t maxSpeed = 300;
uint32_t tsNextReport = 100000; //timestamp to make next report at
const uint32_t tsNextReportInterval = 1000000; //serial reporting interval (microseconds)
int killSwitch = 0; //If parameters are out of spec, turn robot off
int firstNote = 0; //If note played once
//Mode 0 - If robot is upright, it will perform a controlled fall onto its arms. Otherwise, no movement.
//Mode 1 - If robot is upright it will balance. //Not fully implemented. Otherwise, it will (slowly) get onto maximum arm inclination and try to pop up.
uint8_t robotMode = 1;
uint8_t robotModePrevious = 1;
uint8_t runArmAdjust = 0;
double aaAngle[6];
uint32_t armAdjustNext = 0;
const uint32_t armAdjustNextInterval = 50000;
//==============================
//Pin 7 ISR Function
//==============================
void xAxisRCPinISR(){
uint32_t thisTimeStamp = micros();
bool xAxisPinState = digitalRead(xAxisRCPin);
if(xAxisPinState != lastXRCState){
if(xAxisPinState==LOW){
xAxisPulseWidth = thisTimeStamp - lastXrcTimeStamp;
}
lastXrcTimeStamp = thisTimeStamp;
lastXRCState=xAxisPinState;
}
//xAxisPulseWidth=1500; //no rc reciever hooked up, so use center point
}
void yAxisRCPinISR(){
uint32_t thisTimeStamp = micros();
bool yAxisPinState = digitalRead(yAxisRCPin);
if(yAxisPinState != lastYRCState){
if(yAxisPinState==LOW){
yAxisPulseWidth = thisTimeStamp - lastYrcTimeStamp;
}
lastYrcTimeStamp = thisTimeStamp;
lastYRCState=yAxisPinState;
}
//yAxisPulseWidth=1500; //no rc reciever hooked up, so use center point
}
//==============================
//Setup Function
//==============================
void setup()
{
Wire.begin();
Serial.begin (115200);
//imu stuff
if (!imu.init())
{
Serial.println("Failed to detect and initialize IMU!");
while (1);
}
imu.enableDefault();
//calibrate the gyro (2 degrees error observed between accel angle and filtered if not calibrated)
for(int i=0;i<200;i++){
imu.read();
gyroZeroRotationRate = gyroZeroRotationRate + imu.g.y;
}
gyroZeroRotationRate = gyroZeroRotationRate * 245.0 / 32749.0 / 200.0;
filteredAngle = 0;
//other stuff
pinMode(xAxisRCPin, INPUT_PULLUP);
pinMode(yAxisRCPin, INPUT_PULLUP);
motors.flipLeftMotor(true);
motors.flipRightMotor(true);
attachInterrupt(digitalPinToInterrupt(xAxisRCPin), xAxisRCPinISR, CHANGE);
attachInterrupt(digitalPinToInterrupt(yAxisRCPin), yAxisRCPinISR, CHANGE);
lcdPrint();
}
//==============================
//Loop Function ****************
//==============================
void loop()
{
uint32_t currentTime = micros();
//Operations priority
// (1) Check for next imu point
// (3) Robot setdown on arms
// (4) Update motor speeds
// (5) Reporting
//-------------------------------------------
// (1) Check for next IMU point
//-------------------------------------------
if (currentTime>imuNextSample){
//add a imu sample
imu.read();
lastImuTimeStamp = thisImuTimeStamp;
thisImuTimeStamp = currentTime;
//32749 = 245 dps --> 1 dps = 133.67;
gyroRotationRate = imu.g.y * 245.0 / 32749.0;
gyroRotationRate = gyroRotationRate - gyroZeroRotationRate;
filteredAngle = filteredAngle + gyroRotationRate * (thisImuTimeStamp - lastImuTimeStamp)/1000000;
//Serial.print(gyroRotationRate);
//Serial.print(" ");
//Serial.println(gyroRotationRate * (thisImuTimeStamp - lastImuTimeStamp)/1000000.0);
//Serial.print(filteredAngle);
//Serial.print(" ");
xzAngle = atan2(-imu.a.z,imu.a.x);
xzAngle = ((xzAngle / 2.0 / 3.14159 * 360.0) + 180.0 );
filteredAngle = (filteredAngle * 0.925) + (xzAngle * 0.075);
//Serial.print(" ");
//Serial.print(xzAngle);
//Serial.print(" ");
//Serial.println(filteredAngle);
imuNextSample = imuNextSample + imuNextSampleInterval;
}
//-------------------------------------------
// (4) Update motor speeds
//-------------------------------------------
else if (currentTime>pidNextAdjust){
targetAngle = balanceAngle - 9.0 / 500.0 * (double)(yAxisPulseWidth - 1500);
LRSpeedDifference = -25.0 / 500.0 * (double)(xAxisPulseWidth - 1500);
//Update encoder counts, calculate rates, prime variables for the next pass
countsLeft = encoders.getCountsLeft();
countsRight = encoders.getCountsRight();
countRateLeft = ((double)countsLeft - (double)countsLeftLast ) / ((double)currentTime - (double)countsLastTS);
countRateRight = ((double)countsRight - (double)countsRightLast) / ((double)currentTime - (double)countsLastTS);
angleIntegral = 0.95 * angleIntegral + (targetAngle - filteredAngle) * (currentTime - countsLastTS) / 1000;
angleIntegral = angleIntegral * iDecay;
//Calculate errors for different targets
errorEncoderCount = targetEncoderCount - (countsLeft + countsRight)/2.0;
errorEncoderRate = targetEncoderRate - (countRateLeft + countRateRight)/2.0;
errorAngle = targetAngle - filteredAngle;
errorRotationRate = targetRotationRate - gyroRotationRate;
//Calculate the motor speed difference from the error terms
speedDifference = Ki * angleIntegral + Kp * errorAngle + Kd * errorRotationRate;
motorSpeed = speedDifference;
motorSpeed = constrain(motorSpeed, -(int16_t)maxSpeed, (int16_t)maxSpeed);
buzzer.playFrequency(660 + motorSpeed, 11, 9);
if(currentTime < 5000000){
motors.setSpeeds(0, 0);
}
else {
motors.setSpeeds(motorSpeed + LRSpeedDifference, motorSpeed - LRSpeedDifference);
ledGreen(0);
}
//targetAngle = targetAngle - errorEncoderRate * 1;
//targetEncoderCount = 0.99 * targetEncoderCount + (countsLeft + countsRight)/2.0*0.01;
//Clean up state variables
pidNextAdjust = pidNextAdjust + pidNextAdjustInterval;
countsLastTS = currentTime;
countsLeftLast = countsLeft;
countsRightLast = countsRight;
}
//-------------------------------------------
// (5) Reporting
//-------------------------------------------
else if (currentTime>tsNextReport ){
tsNextReport += tsNextReportInterval;
Serial.print(Ki * angleIntegral);
Serial.print(" ");
Serial.print(Kp* errorAngle);
Serial.print(" ");
Serial.print(Kd * errorRotationRate);
Serial.print(" ");
Serial.print(Ki * angleIntegral + Kp* errorAngle + Kd * errorRotationRate);
Serial.print(" ");
Serial.print(yAxisPulseWidth);
Serial.print(" ");
Serial.println(xAxisPulseWidth);
}
if (killSwitch==1 && firstNote == 0){
buzzer.playNote(NOTE_A(4), 750, 10);
firstNote = 1;
}
if(buttonA.getSingleDebouncedPress()){
if(activeK==1)
Ki = Ki * 1.1;
else if(activeK==2)
Kp = Kp * 1.1;
else if(activeK==3)
Kd= Kd * 1.1;
lcdPrint();
//balanceAngle = balanceAngle + 0.1;
//lcd.clear();
//lcd.print(balanceAngle);
}
if(buttonB.getSingleDebouncedPress()){
if(activeK==1)
Ki = Ki / 1.1;
else if(activeK==2)
Kp = Kp / 1.1;
else if(activeK==3)
Kd= Kd / 1.1;
lcdPrint();
//balanceAngle = balanceAngle - 0.1;
//lcd.clear();
//lcd.print(balanceAngle);
}
if(buttonC.getSingleDebouncedPress()){
if(activeK<3)
activeK++;
else
activeK=1;
lcdPrint();
}
}
void lcdPrint(){
lcd.clear();
lcd.print("i");
lcd.print(Ki*1000);
lcd.setCursor(4,0);
lcd.print("p");
lcd.print(Kp*100);
lcd.setCursor(0,1);
lcd.print("d");
lcd.print(Kd*100);
lcd.setCursor(7,1);
if(activeK==1)
lcd.print("i");
else if(activeK==2)
lcd.print("p");
else if(activeK==3)
lcd.print("d");
}
'''
Nathan,
Thank you!
Drew