Motors and encoders arrived so I was able to make a prototype and write some code.
Turns out the Pololu SMC is not able to switch between seral/I2C and analog input without the pc software, so I had to find another way for the manual maneuver mode. Luckily it was easy to implement in arduino thanks to these sources:
https://www.impulseadventure.com/elec/robot-differential-steering.html
Then the code for calculation of steering angle. It is a theoretical steering angle based on the differential speed of the wheels and the vehicle dimensions, some formulas to be found in this source:
The reading of encoder pulse changes since last measurement, dPos:
currentPosL = rotL.read(); //get encoder position, derive pulse speed
currentPosR = -rotR.read(); //inverse mounting
if (currentPosL != lastPosL || currentPosR != lastPosR) {
dPosL = currentPosL - lastPosL;
lastPosL = currentPosL;
dPosR = currentPosR - lastPosR;
lastPosR = currentPosR;
}
else {
dPosL=0;
dPosR=0;
}
Calculation of steer angle based on dPos:
if ((abs(dPosL) < dPosTreshold && abs(dPosR) < dPosTreshold) || (dPosL == dPosR))
{
steerAngleCurrent = 0;
steerAngleActual = 0;
}
else if (dPosL < dPosR)
{
R_turnradius = (trackwidth*0.5)*(dPosL+dPosR)/(dPosR-dPosL);
steerAngleCurrent = -180/PI*atan2(wheelbase, R_turnradius); //result of atan2 is radians, *180/pi gives degrees
}
else if (dPosL > dPosR)
{
R_turnradius = (trackwidth*0.5)*(dPosL+dPosR)/(dPosL-dPosR);
steerAngleCurrent = 180/PI*atan2(wheelbase, R_turnradius);
}
steerAngleActual += steerangle_smoother*(steerAngleCurrent-steerAngleActual); // simple low pass filter
if (watchdogTimer < 10)// && modeSwitch == 1) //autosteer driving
{
steerAngleError = steerAngleActual - steerAngleSetPoint; //calculate the steering error
speedcontrolPID(); //do the motor speed pid
calcSteeringPID(); //do the steer angle pid
motorDrive(); //out to motors the L and R motorspeed
}
else // set to manual if communication is lost or manual mode is selected
{ //we've lost the comm to AgOpenGPS, or just stop request
manualmode();
}
This is the code for 1) a PID to adjust throttle of left and right wheel to achieve the desired speed, and 2) the PID to adjust differential L/R throttle to achieve desired steer angle. Finally 3) the main throttle of the vehicle is adjusted and throttle L/R is sent to the controllers.
I am new to coding and especially PID so probably some things to improve here.
void speedcontrolPID(void) //first adjust the individual throttle of left and right wheels so that their speed is corrected
{
lastthrottle = (throttleL + throttleR)/2; //throttle difference to pick up later = new throttle - last throttle
speedL = dPosL * dPosUnit; //throttle setpoint, speed feedback. Thanks to http://andrewjkramer.net/pid-motor-control/
adjustL = speedKp * (throttleL - speedL) + speedKd * (speedL - lastspeedL); //error is throttle - speed
lastspeedL = speedL;
speedR = dPosR * dPosUnit;
adjustR = speedKp * (throttleR - speedR) + speedKd * (speedR - lastspeedR);
lastspeedR = speedR;
throttleL -= adjustL;
throttleR -= adjustR;
}
//#########################################################################################
void calcSteeringPID(void) //second adjust the differential -p/+p throttle so that steer angle is corrected
{
//Proportional only
pValue = steerSettings.Kp * steerAngleError *steerSettings.Ko;
pDrive = (constrain(pValue, -maxmotorspeed*maxdiffdrive, maxmotorspeed*maxdiffdrive)); //pwmDrive originally was -255,255 so a factor 10 next to Kp, Ko is included. Maxdiffdrive limit for safety
//add min throttle factor so no delay from motor resistance.
if (pDrive < 0 ) pDrive -= (steerSettings.minPWMValue);
else if (pDrive > 0 ) pDrive += (steerSettings.minPWMValue);
if (pDrive > maxmotorspeed*maxdiffdrive) pDrive = maxmotorspeed*maxdiffdrive;
if (pDrive < -maxmotorspeed*maxdiffdrive) pDrive = -maxmotorspeed*maxdiffdrive;
throttleL += pDrive;
throttleR -= pDrive;
}
//#########################################################################################
void motorDrive(void) //third adjust main throttle and out to the motors
{
pDisplay = pDrive;
throttle = map(analogRead(throttle_readpin), 0, 1023, 0, maxmotorspeed);
// throttle = 1600;
pedalbrake = map(analogRead(brake_readpin), 200, 800, 1, 0);
if (pedalbrake < 0) pedalbrake = 0;
if (pedalbrake > 1) pedalbrake = 1;
throttle *= pedalbrake;
throttleL += (throttle - lastthrottle);
throttleR += (throttle - lastthrottle);
if (throttleL > maxmotorspeed) throttleL = maxmotorspeed;
if (throttleL < -maxmotorspeed) throttleL = -maxmotorspeed;
if (throttleR > maxmotorspeed) throttleR = maxmotorspeed;
if (throttleR < -maxmotorspeed) throttleR = -maxmotorspeed;
setMotorSpeed(throttleL, I2C_L); //set left motor speed
setMotorSpeed(throttleR, I2C_R); //set right motor speed
}
Here is a video showing first a demo of the manual mode and then some fiddling with the drive function in Agopengps. It’s a proof of concept but as you can see a lot to improve. Calculated steer angle moves wildly, maybe partly due to mechanics like a loose chain link, probably more due to insufficient code.
First steps are taken, feel free to comment! Next I will probably build a small version of the vehicle that can carry the battery, to check if it can hold a line in the real world.
