I have built a self balancing unicycle with a SyRen motor driver that receives packetized serial commands from an Arduino Uno. When the unicycle leans forward, the power is applied forward to balance. When it leans backwards, power is applied in reverse to balance it. It does work, but the problem is that, randomly(while running), it will become reversed (applying reverse torque when leaned forward, and forward torque when leaned backwards). Because the lean angle just increases when torque is applied the wrong way, it applies a great deal of power, and it goes nuts. It actually dealt quite a blow to my shin when it first happened. If I reset the Arduino it will return to normal. I am pretty new to programming, so it is most likely a code issue. The machine is complete, and this is the only thing that is keeping me from riding it, so any advice is greatly appreciated. Thanks.
Here is the part of my code that pertains to the packetized serial commands. I can't see anything wrong with this, but I'm a newbie, and this is where the problem most likely lies:
void loop()
{
Int_Output = updatePid(BalancePoint, Pitch); //runs PID controller and returns output
SyRen_Output = abs(Int_Output); // number output will always be 0 to 127 (motor conrtoller only accepts positive integers in this range)
if (angles[1]*2 > BalancePoint) { //if pitch angle(x2 to make deadzone smaller) is positive (unicyle is leaned back), motor direction will be reverse
Serial.write(128); //serial address of SyRen motor driver
Serial.write((byte)0); //this determines direction of output
Serial.write(SyRen_Output); //power output
Serial.write((128+(byte)0+SyRen_Output) & 0b01111111); //checksum to confirm correct commands (SyRen will not act on command packet if these values are different)
}
if (angles[1]*2 < BalancePoint) { //same as other packet except direction is forward
Serial.write(128);
Serial.write(1);
Serial.write(SyRen_Output);
Serial.write((128+1+SyRen_Output) & 0b01111111);
}
Here is the entire code is you want to see it:
//code for homemade self balancing unicycle:
//sparkfun 6dof IMU
//PID output control
//Dimension Engineering SyRen 25 motor driver
//Arduino IDE 1.0
#include <FreeSixIMU.h> //fusion filtering library
#include <FIMU_ADXL345.h> //accelerometer library
#include <FIMU_ITG3200.h> //gyro library
#include <Wire.h>
#define GUARD_GAIN 20.0
//timing
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
//for IMU:
float angles[3]; //yaw pitch roll (only pitch is necessary for one-axis balance in this application)
FreeSixIMU sixDOF = FreeSixIMU(); //set the FreeSixIMU object
//for PID controller:
float K = 0.1;
int Kp = 95;
int Ki = 2;
int Kd = 1;
int last_error = 0;
int integrated_error = 0;
int pTerm = 0, iTerm = 0, dTerm = 0;
int error;
int BalancePoint = 0; //the angle at which the PID controller will attempt to balance the unicycle
int Pitch;
//for SyRen
int SyRen_Output; //final motor output value
int Int_Output; //used to convert pid output to int
void setup()
{
Serial.begin(9600); //set baud rate
Wire.begin();
delay(5);
sixDOF.init(); //begin the IMU
delay(5);
Serial.write(170); //initializes SyRen controller and lets it determine baud rate
}
int updatePid(int BalancePoint, int Pitch) { //PID control: gets angles and calculates output from -127 to 127
sixDOF.getEuler(angles);
Pitch = angles[1] * 2;
error = BalancePoint - Pitch;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
return -constrain(K*(pTerm + iTerm + dTerm), -127, 127);
}
void loop()
{
Int_Output = updatePid(BalancePoint, Pitch); //runs PID controller and returns output
SyRen_Output = abs(Int_Output); // number output will always be 0 to 127 (motor conrtoller only accepts positive integers in this range)
if (angles[1]*2 > BalancePoint) { //if pitch angle(x2 to make deadzone smaller) is positive (unicyle is leaned back), motor direction will be reverse
Serial.write(128); //serial address of SyRen motor driver
Serial.write((byte)0); //this determines direction of output
Serial.write(SyRen_Output); //power output
Serial.write((128+(byte)0+SyRen_Output) & 0b01111111); //checksum to confirm correct commands (SyRen will not act on command packet if these values are different)
}
if (angles[1]*2 < BalancePoint) { //same as other packet except direction is forward
Serial.write(128);
Serial.write(1);
Serial.write(SyRen_Output);
Serial.write((128+1+SyRen_Output) & 0b01111111);
}
//Serial.print(SyRen_Output); //for debugging
//loop timing control
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
}