As what I promised, I will post my simply PID(PD) controller code here:
void PID(void)
{
if((leftSensor > hasLeftWall && rightSensor > hasRightWall))//has both walls
{ //ccw direction is positive
errorP = rightSensor – leftSensor – 63;//63 is the offset between left and right sensor when mouse in the middle of cell
errorD = errorP – oldErrorP;
}
else if((leftSensor > hasLeftWall))//only has left wall
{
errorP = 2 * (leftMiddleValue – leftSensor);
errorD = errorP – oldErrorP;
}
else if((rightSensor > hasRightWall))//only has right wall
{
errorP = 2 * (rightSensor – rightMiddleValue);
errorD = errorP – oldErrorP;
}
else if((leftSensor < hasLeftWall && rightSensor <hasRightWall))//no wall, use encoder or gyro
{
errorP = 0;//(leftEncoder – rightEncoder*1005/1000)*3;
errorD = 0;
}
totalError = P * errorP + D * errorD;
oldErrorP = errorP;
setLeftPwm(leftBaseSpeed – totalError);
setRightPwm(rightBaseSpeed + totalError);
}
It works pretty well especially if you don’t have a high resolution encoder or accelerometer as feedback. If you use high resolution encoder to generate your own speed profile, you will need to add sensor error onto rotational error associated with encoder for PID.
Thanks for sharing your code 😉
Well, I am using almost the same function (of course with rotational proflier), but I have a small problem. When my micromouse is moving from wall to no wall position on left or right side it shaking a lot. My varibles “hasLeftWall” etc are a litle bit higher than the values on the center of square. If I made them lower I am afraid that my micromouse loose contact with wall and in consequence crash somewhere.
Sorry for my english ;]
Hi,
Since you use rotational speed profile I think you mouse should be able to go straight by only use encoder.During the transition from wall to no wall or no wall to wall, the reading for post is always fading slowly, which cause such a shaking issue as you interpreted. What I did is to use diagonal sensors instead when side sensors are around post area, then switch back to side sensors when side sensors leave post area(this is for my 6 sensor mouse) if you have 4 sensors, basically you just don’t use the sensor value when around post area. Use your encoder to keep track of your position to decide where you should use side sensor values or not. I hope this will help.
me podrias ayudar estoy implementando mi micromouse
Thanks for your replay. I have 4 senosors and turning them off seems to be a good idea. It’s my first mouse and right now I’m trying to deal with PID parameters. But recently I found a good article (by Peter Harrison) about phase lead controller. Do you think that using phase lead controller for correctiong heading/offset error is better idea than a PD ? Right now I am using it for my DC motors and it works better but it’s realy hard to set it’s parameters.
Cheers
HI,
you don’t have to turn off the sensor, you just don’t use the value, because you still need side sensor value to calibrate your mouse’s longitude position during the wall-to-no-wall transition process. I am kinda using phase lead controller now, and you might still need to calculate the position error by using PD controller because as what I understood, phase lead controller is predicting current speed and the position error equals to predicted current speed subtract with encoder feedback. Phase lead controller will make your mouse move much more smoother but you still need to add your sensor error(need to scale it down) to rotational error. you still do PD computation once you accumulate both error from sensor and the error came from (predicted current speed – encoder feedback). I hope this won’t confuse you 🙂
Well right now I have this done like this:
1. I am calculating error from sensors (left & right) using method above (PD) and I am adding it (scaled) to varible “TURN_DISTANCE”
2. Next some other corrections wall to no-wall, forward error by front sensors
3. Now I calculate my desired speed by profiler
4. Finally phase lead controller, and for rotation I have something like that:
error_old_r=error_r;
error_r=TURN_DISTANCE – (encoder_left – encoder_right);
output_old_r=output_r;
output_r = (K1*error_r – K2*error_old_r + K3*output_old_r + 128)/256;
and right now just simply :
PWM_left = output_straight+output_r;
PWM_right = output_straight-output_r;
As I undarstand now it’s wrong… I thoutght that it’s posible to just use phase lead controler instead PID. If it’s only predicting current speed I assume “error_r” should be only equal TURN_DISTANCE ? And than I should use PD where error = output-r – (encoder_left – encoder_right); ? I am using AVR MCU (well, right now I know that it wasn’t good choice) so it’s quite hard to do many calculations.
For sensors of course I meant not using values 🙂
Thanks for your great advices and maybe we should move this discussion to forum ? 😉
if your output_r is the predicted speed value, then total error should be error_r-(speed feedback difference between left and right encoder). Trying not to add all kinds of error together at same time, what I did is only use phase lead controller to predict speed and only use the feedback from encoder to calculate error to make mouse go straight by encoder first. If it works, then I try to make mouse accelerate and decelerate by setting up target transitional speed as desired. I added sensor error to rotational error after both above worked. adding sensor error to rotational error only change the total amount of error and you still need to use this total error to do PD computation afterwards. It’s like you use both encoder and sensor to make your mouse go straight and before you use your sensor to assist your mouse, you use encoders only to make mouse go straight :). Also pay attention to the sign of you errorP and errorD. Since I use right encoder minus left encoder speed to get feedback error, so the sign for my equation might not be different with yours.
Pingback: A simple way to avoid post sensor reading fading issue. | MicromouseUSA
hi:
errorP = 2 * (leftMiddleValue – leftSensor);
that has meaning or value is variable and because leftMiddleValue multiply x2.
rightEncoder * 1005/1000) * 3
because you multiply 1005/1000 and then multiply x3
Hi,
I have facing some gyro problems. Did you know how to solve the gyro drift? Please reply me through email (yukiko_tohmeiqi@hotmail.com). Thanks.