Hi,
This is my first post in eevblog, I’m working on a Line following robot, I’m using stm32G030K as the main controller, and I have connected 4 reflective sensors for the line position and two motor drivers for my motors, I have tried a lot to tune and write a good code so that my robot would do it’s best to follow the line and have good speed, But if I increase my motor speeds it would not follow the line Here is the main parts of the code that would do the job,
checkout the robot in action
https://www.youtube.com/shorts/eJNDz7MstAsint MyPID_kp =50, MyPID_ki = 0, MyPID_kd = 2;
Calibration();
while(1){
MyPID_SetOutputLimits(-5000, 5000);/*Limt my PID output*/
Input = Calculate_LinePosition(); /* calculate Line Position from IR Sensor*/
Output = MyPID(MyPID_kp, MyPID_ki, MyPID_kd,Input);/* calculate PUD parameter*/
RunMotors(Output); // send PID output to PWM for running Motor*/
}
After reset I would calibrate the sensors like this
void Calibration()
{
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); // left
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // right
Tm0 = HAL_GetTick();
Tm1 = Tm0;
unsigned long TmL;
for (uint8_t i = 0; i < IR; i++)
{
SenX[i] = ReadSensors(i);
MinX[i] = SenX[i];
MaxX[i] = SenX[i];
}
while ((HAL_GetTick() - Tm0) <= 2000)
{
for (uint8_t i = 0; i < IR; i++)
{
SenX[i] = ReadSensors(i);
if (SenX[i] < MinX[i])
MinX[i] = SenX[i];
if (SenX[i] > MaxX[i])
MaxX[i] = SenX[i];
}
TmL = HAL_GetTick();
if ((TmL - Tm1) >= 100)
{
Tm1 = TmL;
}
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET);
TIM1->CCR3 = 2500; // right
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_SET);
TIM1->CCR4 = 500; // left
}
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // left
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // right
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET);
TIM1->CCR3 = 0; // right
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET);
TIM1->CCR4 = 0; // left
}
the MyPID_SetOutputLimits would limit the PID output
/*Limit my PID output*/
void MyPID_SetOutputLimits(int Min, int Max)
{
if(Min > Max) return;
outMin = Min;
outMax = Max;
if(Output > outMax) Output = outMax;
else if(Output < outMin) Output = outMin;
if(ITerm> outMax) ITerm= outMax;
else if(ITerm< outMin) ITerm= outMin;
}
I have used matlab line interpolation for finding polynomials to calculate the line position here is the function code
int Calculate_LinePosition(void)
{
Line1 = LinePosition;
for (int i = 0; i < IR; i++)
{
SenX[i] = ReadSensors(i);
// SenX[i] = Map_Sensors(SenX[i], MinX[i], MaxX[i], 800, 3100);
SenX[i] = Map_Sensors(SenX[i], MinX[i], MaxX[i], targetMin, targetMax);
SenX[i] = Constrain_Sensors(SenX[i], targetMin, targetMax);
}
minimum = SenX[activeSensor];
if (SenX[1] < 3000 || SenX[2] < 3000 || SenX[3] < 3000)
{
for (int i = 0; i < IR; i++)
{
if (SenX[i] < minimum)
{
minimum = SenX[i];
activeSensor = i;
}
}
}
// We obtain P1,P2,P3,P4 from basic fitting in matlab
// SenX[0]=SenX[0]+700;
if (activeSensor == 0)
{
if (SenX[1] > 2900)
{
p1 = -0.010884;
p2 = 35.9846;
}
else
{
p1 = 0.0092604;
p2 = 23.3772;
}
}
else if (activeSensor == 1)
{
if (SenX[0] < 2000)
{
p1 = -0.0069286;
p2 = 51.6287;
}
else
{
p1 = 0.0069076;
p2 = 39.8585;
}
}
else if (activeSensor == 2)
{
if (SenX[1] < 3000)
{
p1 = -0.0061269;
p2 = 70.4583;
}
else
{
p1 = 0.0072529;
p2 = 61.0744;
}
}
else if (activeSensor == 3)
{
if (SenX[2] < 2800)
{
p1 = -0.010154;
p2 = 95.2504;
}
else
{
p1 = 0.0075137;
p2 = 78.6127;
}
}
LinePosition = (p1 * SenX[activeSensor] + p2);
if (SenX[0] > 3000 && SenX[1] > 2800)
{
// activeSensor = 0;
}
if (10 < LinePosition && LinePosition < 50)
{
direction = 0;
}
else if (50 < LinePosition && LinePosition < 90)
{
direction = 1;
}
if (LinePosition > 90)
{
LinePosition = 100;
}
if (runLineRobot == false)
{
}
else
{
// if (SenX[0]>1000 && SenX[0]<2000 && SenX[1]>2000&& SenX[2]>1500){
// LinePosition=50;
// MODE_LEFT=true;
}
if (SenX[0] > 2000 && activeSensor == 0)
{
if (direction == 0)
{
LinePosition = 0;
}
if (direction == 1)
{
LinePosition = 100;
}
}
return LinePosition;
}
the actual PID function
int MyPID(int Kp, int Ki, int Kd,int input){
long P = 0, I = 0, D = 0, PIDv = 0, pErr = 0;
MyPID_error=Setpoint - input;
P = MyPID_error;
I = I + MyPID_error;
D = MyPID_error - pErr;
PIDv = 3 * ((Kp * MyPID_error) + (Ki * (I + MyPID_error)) + (Kd * (MyPID_error - pErr)));
pErr = MyPID_error;
return PIDv;
}
The function that would run the motors
void RunMotors(int16_t OutPutPID)
{
if (AllowMotorsRun)
{
PIDValue = mapOutPutPID(OutPutPID, -5000, 5000, -1500, 1500);
if (OutPutPID > 0)
{ // left
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // right
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // left
MotorLeft = 0 - PIDValue; // the power level for the A motor
MotorRight = 0 - PIDValue; // the power level for the C motor
SpeedLeft = BaseSpeed - PIDValue;
SpeedRight = BaseSpeed + PIDValue;
}
else if (OutPutPID < 0)
{ // Right
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // right
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // left
MotorLeft = PIDValue; // the power level for the A motor
MotorRight = PIDValue; // the power level for the C motor
SpeedLeft = BaseSpeed - PIDValue;
SpeedRight = BaseSpeed + PIDValue;
}
else
{
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3); // left
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_4); // right
SpeedLeft = BaseSpeed;
SpeedRight = BaseSpeed;
}
if (SpeedLeft < SpeedMin)
SpeedLeft = SpeedMin;
if (SpeedLeft > SpeedMax)
SpeedLeft = SpeedMax;
if (SpeedRight < SpeedMin)
SpeedRight = SpeedMin;
if (SpeedRight > SpeedMax)
SpeedRight = SpeedMax;
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3); // rifht
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); // left
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_6, GPIO_PIN_RESET); // right
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4, GPIO_PIN_RESET); // left
BATTERY_VOLTAGE = BAT_GetVoltag(adcData[IR]);
// RPM=(BATTERY_VOLTAGE-350)*0.975+150;
// TIM1->CCR4 = SpeedLeft-(BATTERY_VOLTAGE-350)*0.6; // Left
// TIM1->CCR3 = SpeedRight-(BATTERY_VOLTAGE-350)*0.6; // Left
TIM1->CCR4 = SpeedLeft; // Left
TIM1->CCR3 = SpeedRight; // Right
}
}
I'm out of Ideas and I do not know what I have done wrong, do you have any Idea what I should do?