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");
}
'''