void moveMotor(Command motor, double speedRaw) {
static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
// Speed is a value in percentage 0-100%
if (speedRaw > 100)
speedRaw = 100;
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 255) ;
if (motor == left) {
if ((speed == 0) || ((LastOutputLeft > 0) != (speed > 0))) {
motorCoast(left);
}
LastOutputLeft = speedRaw; // Store for Direction Change
// Now we can set the speed
setPWM(45, speed); // Left motor pwm
if (speedRaw > 0) {
cbi(PORTA, PINA2);
sbi(PORTA, PINA3);
}
if (speedRaw < 0) {
sbi(PORTA, PINA2);
cbi(PORTA, PINA3);
}
}
else if (motor == right) {
if ((speed == 0) || ((LastOutputRight > 0) != (speed > 0))) {
motorCoast(right);
}
LastOutputRight = speedRaw; // Store for Direction Change
// Now we can set the speed
setPWM(44, speed); // Right motor pwm
if (speedRaw > 0) {
cbi(PORTA, PINA0);
sbi(PORTA, PINA1);
}
if (speedRaw < 0) {
sbi(PORTA, PINA0);
cbi(PORTA, PINA1);
}
}
}
noMoreCoast(left);
noMoreCoast(right);
/* Set PWM Values */
moveMotor(left, PIDLeft);
moveMotor(right, PIDRight);
void motorCoast(Command motor){
//digitalWrite(DIAG1, LOW);
//digitalWrite(DIAG2, LOW);
if (motor == left) {
}
else {
}
}
void noMoreCoast(Command motor){
//digitalWrite(DIAG1, HIGH);
//digitalWrite(DIAG2, HIGH);
if (motor == left) {
}
else {
}
}
What do you think? Does code respect your sequence?
In noMoreCoast() I am not able to write en/diag HIGH in bit.
speedRaw is a value that comes directly from your PID loop. it will contain a value from(-100% ~ 100%) allowing the value to pass through zero.
Use a something in the PID loop to limit it within this range and prevent windup.
so the differenct is speedRaw is a percentage that can be + or - and speed is an unsigend byte that ranges from Minimum torque set at 30 for now to 255
I would suggest changeing up your PID loop to use a value from -255 to +255 to give you a higher resolution unless you are using a floating point # to represent the % giving you a higher resolution.
void moveMotor(Command motor, double speedRaw) {
static double LastOutputLeft, LastOutputRight; // used to check for motor direction change
// Speed is a value in percentage 0-100%
// if (speedRaw > 100) //constrain below resolves this
// speedRaw = 100;
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 100, 30, 255) ; // adjusted map to meet your 100% range. Note that PWM min is 30 (minimum motor torque)
if (motor == left) {
if ((speedRaw== 0) || ((LastOutputLeft > 0) != (speedRaw> 0))) { // needs to be looking at speedRaw to see transition through zero from positive value to negitive value
motorCoast(left);
}
LastOutputLeft = speedRaw; // Store for Direction Change
// Now we can set the speed
setPWM(45, speed); // Left motor pwm
if (speedRaw > 0) {
cbi(PORTA, PINA2);
sbi(PORTA, PINA3);
}
if (speedRaw < 0) {
sbi(PORTA, PINA2);
cbi(PORTA, PINA3);
}
}
else if (motor == right) {
if ((speedRaw== 0) || ((LastOutputRight > 0) != (speedRaw> 0))) {
motorCoast(right);
}
LastOutputRight = speedRaw; // Store for Direction Change
// Now we can set the speed
setPWM(44, speed); // Right motor pwm
if (speedRaw > 0) {
cbi(PORTA, PINA0);
sbi(PORTA, PINA1);
}
if (speedRaw < 0) {
sbi(PORTA, PINA0);
cbi(PORTA, PINA1);
}
}
}
noMoreCoast(left);
noMoreCoast(right);
/* Set PWM Values */
moveMotor(left, PIDLeft);
moveMotor(right, PIDRight);
void motorCoast(Command motor){
//digitalWrite(DIAG1, LOW);
//digitalWrite(DIAG2, LOW);
if (motor == left) {
}
else {
}
}
void noMoreCoast(Command motor){
//digitalWrite(DIAG1, HIGH);
//digitalWrite(DIAG2, HIGH);
if (motor == left) {
}
else {
}
}
I know I need to constrain() PID and ITerm values, but I have a question.
Look at this post:
"you can set the Output Control Register to a value between 0 and 1599 to get 1600 values of PWM and 10 kHz...."
I've changed Timer5 mode setting the prescaler to 1, so 800 different pwm values (0 - 799).
TCCR5A = B00101001; // Phase and frequency correct PWM change at OCR5A
TCCR5B = B10001; // System clock
OCR5A = 800; // 16MHz/10kHz/2=800 prescaler a 1 (0-799)
OCR5B = 0; // Motors stop D45
OCR5C = 0; // D44
So can I change
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 255) ;
to
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 800) ;
or I am completely wrong? Is my PWM (-799 ; +799)?
Yes more resolution the better the pid loop can fine a better spot to tune to
Now the 30 may be way before the motor starts turning at a minimum speed with no load should be the starting point for the 30 value
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 800) ;
else if (motor == right) {
if ((speedRaw== 0) || ((LastOutputRight > 0) != (speedRaw> 0))) {
motorCoast();
}
LastOutputRight = speedRaw; // Store for Direction Change
noMoreCoast();
// Now we can set the speed
setPWM(44, speed); // Right motor pwm
if (speedRaw > 0) {
cbi(PORTA, PINA0);
sbi(PORTA, PINA1);
}
if (speedRaw < 0) {
sbi(PORTA, PINA0);
cbi(PORTA, PINA1);
}
}
}
void motorCoast(){
digitalWrite(35, LOW);
}
void noMoreCoast(){
digitalWrite(35, HIGH);
}
is noMoreCoast(); in the right place?
is noMoreCoast(); in the right place?
be sure the motor direction is correct before enabling the motors
I moved it to right after the directions are set.
if you continuously call noMoreCoast(); to set motors to the enabled state at each cycle it shouldn't be a problem
else if (motor == right) {
if ((speedRaw== 0) || ((LastOutputRight > 0) != (speedRaw> 0))) {
motorCoast();
}
LastOutputRight = speedRaw; // Store for Direction Change
// Now we can set the speed
setPWM(44, speed); // Right motor pwm
if (speedRaw > 0) {
cbi(PORTA, PINA0);
sbi(PORTA, PINA1);
}
if (speedRaw < 0) {
sbi(PORTA, PINA0);
cbi(PORTA, PINA1);
}
noMoreCoast(); // make sure direction is set correctly before enabling motors
}
}
void motorCoast(){
digitalWrite(35, LOW);
}
void noMoreCoast(){
digitalWrite(35, HIGH);
}
@zhomeslice NOW that you have explained me very patiently noMoreCoast(); function I am very happy to say that the robot smoothly corrects for 4-5sec then overshoots in a direction. Do I need to tune Kd or add a little of Ki (that now is zero)?
look at the video.. at ZERO error (degree) the motor driver is OFF, but it seems slow in changing direction... so I don't know if the LastOutput is working... what do you think?
if ((speedRaw== 0) || ((LastOutputLeft > 0) != (speedRaw> 0))) {
void PID(double restAngle, double offset, double turning, double dt) {
/* Update PID values */
double error = restAngle - pitch;
double pTerm = Kp * error;
iTerm += Ki * error * dt;
if(iTerm> 100) iTerm= 100;
else if(iTerm< -100) iTerm= -100;
double dTerm = Kd * (error - lastError)/dt;
lastError = error;
double PIDValue = pTerm + iTerm - dTerm;
if(PIDValue > 799) PIDValue = 799;
else if(PIDValue < -799) PIDValue = -799;
/* Steer robot sideways */
double PIDLeft;
double PIDRight;
PIDLeft = PIDValue;
PIDRight = PIDValue;
// PIDRight *= 0.95; // compensate for difference in the motors
/* Set PWM Values */
moveMotor(left, PIDLeft);
moveMotor(right, PIDRight);
}
/* PID variables */
double Kp = 10; // 10
double Ki = 0; // 0.000001
double Kd = 5; // 5
double targetAngle = 0;
I'm trying to make the robot starts to balance automatically when angle vertical error goes under 1.
What do you think?
/* Calculate pitch */
pitch = ypr[1] * 180/M_PI; // Calculate the angle using DMP library
if(abs(pitch) > 1)
{
return;
}
else if(abs(pitch) < 1)
{
/* Drive motors */
timer = micros();
PID(targetAngle,targetOffset,turningOffset,(float)(timer - pidTimer) / 1000000.0f);
pidTimer = timer;
/* Update encoders */
timer = millis();
if (timer - encoderTimer >= 100) { // Update encoder values every 100ms
encoderTimer = timer;
wheelPosition = readLeftEncoder() + readRightEncoder();
wheelVelocity = wheelPosition - lastWheelPosition;
lastWheelPosition = wheelPosition;
if (abs(wheelVelocity) <= 40 && !stopped) { // Set new targetPosition if braking
targetPosition = wheelPosition;
stopped = true;
}}
}
ROBOT VIDEO ADDED
The first thing I see that is easily corrected you mention here
but it seems slow in changing direction...
The line of code looks something like this that you posted earlier
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 800) ;
// I'm altering the code to explain a value
int MinMotorSpeed = 200;
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 800, MinMotorSpeed, 800) ;
OK the first 0,800 is the range of your PID loop because we use the absolute value of speedRaw the PID loop can run from -800 (full reverse) to 800 (full forward).
We now need to adjust it to match the torque of the motors we are driving.
We are looking at: MinMotorSpeed, 800
If you manually set the PWM of the motors to 100 (PWM range is 0-800) does the motor start to turn without any load? if not we need to increase the MinMotorSpeed to 200 otherwise we may need to decrease it.
If the motors can't instantly reverse then we will lag in changing directions.
in conclusion the map function adjusts the speed as follows
-800 = 800 (Full PWM)
0 = 200 (minimum PWN where the motor still turns barely under little to no load)
800 = 800 (Full PWM)
PID tuning:
I found that starting with proportional only ( it will not balance )
P only will cause oscillation. Adjust it till it has a slow oscillation you will need to assist your robot. too little and it won't reach the top of the swing, too much and it snaps to the other side fast
integral is more for correcting error from set-point. since setpoint balance is zero and motor Minimum is zero PID output then integral has little relevance. Having a little integral in there could assist in recovery though. now derivative is rate based. It asks "are we approaching setpoint too fast?" then derivative kicks in and can even reverse the motors to slow and land on balance setpoint. Too much and it will over correct again not allowing balance.
I'll look over the code again later to see if I spot anything else and post more if i do.
/* Drive motors */
// If the robot is laying down, it has to be put in a vertical position before it starts balancing
// If it's already balancing it has to be ±45 degrees before it stops trying to balance
if((layingDown && (pitch < 170 || pitch > 190)) || (!layingDown && (pitch < 135 || pitch > 225))) {
layingDown = true; // The robot is in a unsolvable position, so turn off both motors and wait until it's vertical again
stopAndReset();
}
else {
layingDown = false; // It's no longer laying down
PID(targetAngle,targetOffset,turningOffset);
}
Can you explain me better this line please?
if((layingDown && (pitch < 170 || pitch > 190)) || (!layingDown && (pitch < 135 || pitch > 225))) {
Keep it in lying down mode until it goes back to almost vertical like 2° off of vertical.
Also make your PID loop go dormant or reset it as it gets to vertical. We are Trying to avoid something called wind up. Intergal portion of the PID loop is additive. and so the longer it is off of setpoint the more alterations it tries to make to correct for the deviation.
So after this sequence:
if((layingDown && (pitch < 178 || pitch > 182)) || (!layingDown && (pitch < 135 || pitch > 225))) {
The first part is checked when it is laying down it tests to see it get to vertical before it starts controlling the balance.let it get to vertical +-2 degrees
The second part cancels the balance preventing a runaway condition.
Also 10 degrees of of setpoint is too much let's start 2 degrees and make it easier to tune our PID loop
ok so 1-2 degrees...
x=abs(pitch);
if((layingDown && (x < 1 || x > 1)) || (!layingDown && (x < 45 || x > 45))) {
This looks correct.
The code I'm using that is similar to yours I posted a version earlier:
// Input is in +- degrees from vertical of the balancing from my MPU6050 FIFO buffer
I = abs(Input); // my input swings threw 0 degrees so negative numbers and positive numbers with zero as balanced.
If the gyro is off you will find that you may fall to one side all the time
if(I > 45)
{
myPID.SetMode(MANUAL); // Stop the PID calculations prevents windup
//Serial.println("bang fallen over");
Output = 0; //stops the motors
return;
}
else if(I < 1) // I'm waiting till 1 degree off of vertical
{
myPID.SetMode(AUTOMATIC); // Start the PID Calculations prevents windup
//Serial.println("balancing");
}
Just managed to make a decent video at mpu6050 imu arduino pid problems - YouTube
my parameters are
/* PID variables */
double Kp = 10; // 10
double Ki = 0; // 0
double Kd = 10; //10
why is so unstable? 
coast_mode.ino (13.7 KB)
/* PID variables */
double Kp = 10; // 10
double Ki = 0; // 0
double Kd = 0; //change to zero
What does it do with proportional only?
The derivative value seems to be to too aggressive. It sees it falling and over powers the recovery.
very strange.
with only kp it seems to recover better and do not overshoot.
Try less proportional and see if it's still oscillates.
Keep on weakening proportional until fails to stand back up. Increased proportional until it just starts oscillating again.
Then slowly add derivative .5 at a time
/* PID variables */
double Kp = 9; // 9
double Ki = 0.2; //0.2
double Kd = 8; // 8-10
This seems the best settings so far, but it is not able to balance for more than 2 seconds because of backlash(??)
don't really know...
2 things cause backlash
Kd
and the motor minimum power
the minimum power is represented by the number 30 int this equation your is now defferent
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 255) ;
send me your PID code portion and I will test it on my robot.
zhomeslice:
2 things cause backlash
Kd
and the motor minimum power
the minimum power is represented by the number 30 int this equation your is now defferent
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 255, 30, 255) ;
send me your PID code portion and I will test it on my robot.
int speed = map(constrain (abs (speedRaw) , 0, 100) , 0, 100, 85, 799) ; //Note that PWM min is 30on255 (minimum motor torque)so 85on800
//800 is my pid increased resolution
int speed = map(abs (speedRaw) , 0, 799, 90, 799); This is better because I do not need constrain() because I have not already added turning code. right?
def.ino (10.1 KB)
correct.. there is one other thing that causes backlash I discovered.
It is when the timing delta T is not calculated correctly!
The MPU sends the interrupt at about x seconds it is somewhat fixed. The DMP data from the FIFO buffer could be corrupt and we skip it or we get delayed and such so in conclusion we can't assume the data is at a fixed interval, we may have to wait
so the Arduino Playground - PIDLibrary is flawed because it requires a fixed time that all calculations are based off of. this is not a problem if we can instantly capture a temperature form an analog input. but when it comes to a delay of about 10 milliseconds between readings we need to run the PID routine at the moment we get the new values and the time may not be exactly 10 milliseconds.
The following code is NOT been fully tested I just made these changes last night!
I will post the full code after testing I hope within roughly 24 hours.
// altered these function in the pid_v1.cpp
bool PID::Compute()
{
if (!inAuto) return false;
unsigned long now = millis();
unsigned long timeChange = (now - lastTime);
if (timeChange >= SampleTime)
{
// kp - proportional gain
// ki - Integral gain
// kd - derivative gain
// dt - loop interval time
// outMax - maximum value of manipulated variable
// outMin - minimum value of manipulated variable
//Compute all the working error variables
double input = *myInput;
// Calculate error
double error = *mySetpoint - input;
// Proportional term
double PTerm = kp * error;
// Integral term
_integral += error * (double) (timeChange * .001);
if (_integral > outMax) _integral = outMax;
else if (_integral < outMin) _integral = outMin;
ITerm = ki * _integral;
// Derivative term
double derivative = (error - _pre_error) / (double)(timeChange * .001);
double DTerm = kd * derivative;
//Compute PID Output
double output = PTerm + ITerm + DTerm;
if (output > outMax) output = outMax;
else if (output < outMin) output = outMin;
*myOutput = output;
//Remember some variables for next time
lastInput = input;
/*
for (static long QTimer = millis(); (long)( millis() - QTimer ) >= 0; QTimer += 100 ) { // one line display delay timer waits 100 miliseconds
Serial.print("\t Input"); Serial.print(input);
Serial.print("\t PTerm"); Serial.print(PTerm);
Serial.print("\t ITerm"); Serial.print(ITerm);
Serial.print("\t DTerm"); Serial.print(DTerm);
Serial.print("\t DeltaT"); Serial.print((double)(timeChange * .001));
Serial.print("\t Output"); Serial.print(output);
Serial.println();
}
*/
lastTime = now;
return true;
}
else return false;
}
void PID::SetTunings(double Kp, double Ki, double Kd)
{
if (Kp < 0 || Ki < 0 || Kd < 0) return;
dispKp = Kp; dispKi = Ki; dispKd = Kd;
double SampleTimeInSec = ((double)SampleTime) / 1000;
kp = Kp;
ki = Ki ;
kd = Kd ;
if (controllerDirection == REVERSE)
{
kp = (0 - kp);
ki = (0 - ki);
kd = (0 - kd);
}
}
// added these to the pid_v1.h file
double _dt;
double _pre_error;
double _integral;
NXTfreeDOmF I will also make a replacement function that will allow your code to use this.
It will be easy to insert into your code I just need a short testing this evening and I will make it compatible for your sketch. 