Thanks for getting back to me. I was under the impression that maybe it was a code line that I am not aware of. Here is my code, it is very extensive as this is my first time using an arduino so it is not optimised at all. The robot works well, I am just not able to update my PID values via the serial monitor. I have included the main, the PID file and the Tuning file as I cant upload it all due to character restriction.
NL SHEAD Balancing Robot Main file
#include <SoftwareSerial.h> // Serial to bluetooth
#include <math.h>
#include <TinkerKit.h>
TKGyro gyro(I2, I3, TK_X4); // creating the object 'gyro' that belongs to the 'gyro' class
// and giving the values to the desired input pins
TKAccelerometer accelerometer(I4, I5); // creating the object 'accelerometer' that belongs to the 'TKAccelerometer' class
// and giving the values to the desired input pins
SoftwareSerial mySerial(4, 7); // RX, TX
/******** Motor Variables *********/
#define PWMA 3
#define PWMB 11
#define DIR_A 12
#define DIR_B 13
#define GUARD_GAIN 20.0
unsigned long lastMilli = 0; // loop timing
unsigned long lastMilliPrint = 0; // loop timing
long count = 0; // rotation counter
long countInit;
long tickNumber = 0;
boolean run = false; // motor moves
/******** Timing Variables *********/
unsigned long time;
int sampleTime = 10;
int STD_LOOP_TIME = 9;
int lastLoopTime = STD_LOOP_TIME;
int lastLoopUsefulTime = STD_LOOP_TIME;
unsigned long loopStartTime = 0;
/******** Accelerometer Variables *********/
int xAxisValue = 0; // a variable to store theaccelerometer's x value
int yAxisValue = 0; // a variable to store theaccelerometer's y value
float Vref = 3.33; //Reference VOltage
float V_x;
float V_y;
float V_xzero = 0.99;
float V_yzero = 1.65;
float deltaVolts_x;
float deltaVolts_y;
float sensitivity_ACC = 0.3;
float X_accel;
float Y_accel;
float R;
float X_accel_angle;
float Y_accel_angle;
float X_accel_degrees;
float Y_accel_degrees;
float Theta_accelerometerX;
float Theta_accelerometerY;
float AccAngle;
/******** Gyroscope Variables *********/
float rollADC, pitchADC;
float rollZeroADC = 0, pitchZeroADC = 0;
float rollRate, pitchRate;
float prev_rollRate = 0, prev_pitchRate = 0;
float rollAngle = 0, pitchAngle = 0;
float GyroPitchRate;
/******** Accuracy Variables *********/
float correctionX = 0, correctionY = 0;
float Threshold = 5;
float cal = 100;
float sensitivity = 0.2077;
/******** Filter Variables *********/
float Theta = 0;
int error = 0;
/******** PID and Drive Variables *********/
float setPoint = 0;
int drive = 0;
//int iTerm;
float Theta_gyro;
void setup()
{
Serial.begin(115200); //Initialise serial bps
pinMode(DIR_A, OUTPUT); // Direction pin on channel A
pinMode(DIR_B, OUTPUT); // Direction pin on channel B
delay(100);
GyroCalibrate();
mySerial.begin(115200); //initialise bluetooth
mySerial.println("Online"); //show bluetooth online
}
void loop()
{
/***************** SENSORS ***********************************/
GyroPitchRate = GetGyroRate();
AccAngle = GetAccAngle();
/***************** Filter ***********************************/
Theta = CompFilter(AccAngle,GyroPitchRate,lastLoopTime);
/********** Controller and Drive ***************************/
drive = updatePid(setPoint, Theta); // PID algorithm
if(Theta>(setPoint-30) && Theta<(setPoint+30))
Drive_Motor(drive);
else
Drive_Motor(0); // stop motors if situation is not recoverable
/***************** PID Tuning ***********************************/
getTuning();
/********************** LOOP TIMING CONTROL **************************/
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME)
delay(STD_LOOP_TIME-lastLoopUsefulTime);
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
/***************** Serial Output ***********************************/
// serialOut_timing();
//Serial.print(time);
//Serial.print(",");
//Serial.print(Theta);
//Serial.print(",");
//Serial.print(Y_accel_angle);
//Serial.print(",");
//Serial.print(Theta_gyro);
//Serial.print(",");
//Serial.print(Theta_comp);
//Serial.print("\n");
}
NL SHEAD PID CONTROLLER file
#define GUARD_GAIN 20.0
float K = 1; //Gain factor
float Kp = 190; //Proportional Gain
float Ki = 0; //Integral Term
float Kd = 6; //Derivative Gain
int last_error = 0;
int integrated_error = 0;
int pTerm = 0, iTerm = 0, dTerm = 0;
int ierror=0;
int updatePid(int SetPoint, int Theta)
{
error = SetPoint - Theta;
pTerm = Kp * error;
integrated_error += error;
iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
dTerm = Kd * (error - last_error);
last_error = error;
ierror = constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
// Serial.print(ierror);
// Serial.print("\n");
return -constrain(K*(pTerm + iTerm + dTerm), -255, 255);
}
NL SHEAD TUNING file
int getTuning()
{
if(!mySerial.available()) return 0;
delay(10);
char param = mySerial.read(); // get parameter byte
if(!mySerial.available()) return 0;
char cmd = mySerial.read(); // get command byte
mySerial.flush();
switch (param)
{
case 'p':
if(cmd=='+') Kp+=1;
break;
if(cmd=='-') Kp-=1;
break;
case 'i':
if(cmd=='+') Ki+=0.5;
if(cmd=='-') Ki-=0.5;
break;
case 'd':
if(cmd=='+') Kd+=2;
if(cmd=='-') Kd-=1;
break;
case 'k':
if(cmd=='+') K +=1;
if(cmd=='-') K -=0.5;
break;
default:
mySerial.print("?"); mySerial.print(param);
mySerial.print("?"); mySerial.println(cmd);
}
mySerial.println();
mySerial.print("K:"); mySerial.print(K);
mySerial.print(" Kp:"); mySerial.print(Kp);
mySerial.print(" Ki:"); mySerial.print(Ki);
mySerial.print(" Kd:"); mySerial.println(Kd);
}
Apologies again for so much code, but maybe it will help solve this issue.
Thanks,
Nick
