Balboa examples

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

1 Like

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

2 Likes

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