Jamie’s example gets you closer to what you want to experiment with.
I have another quick note on your code:
signed char distance = p1_y - p2_y;
Will result in a compile error because the declaration of a variable (“signed char distance”) is fixed at compile time, and the value assigned can only be a constant. Since in this case p1_y & p2_y are variables and can be different each time the routine is called, to get past the compiler this would need to be expressed as:
signed char distance;
distance = p1_y - p2_y;
The first statement is evaluated at compile time (on your PC) and the second will be evaluated at run time (when your code is running in the controller).
Your method of correcting will expose another problem with driving straight.
The right/left drive trains are rarely 100% balanced. Manufacturing differences in the motors, team fabrication differences in the friction between the gears/sprockets, chains, slight binding and misalignment, the left/right balance of the robot, and so on. This usually means that to drive straight the motors on one side might get a pwm of say 200, while the motors on the other side need to get 180.
Now a driver corrects for this automatically through a little practice with the robot by pushing the throttle on one side a little more than the other until the robot is seen to be driving straight. If you implement your proposed scheme the code will override this minor correction on the part of the driver and it might actually be harder to drive straight.
(I have an ulterior motive here to introduce you to a primitive form of proportional correction, the first step into PID.)
An easy method is to slow the more powerful side down to match the less powerful side as a percentage of full power, e.g., if p1_y is positive…
stronger side power = p1_y - (p1_y/127 * constant)
weaker side power = p2_y
The simplest way to select constant is:
- Run the robot at full power and note which side lags.
- Pick a value for constant above, put it into the code and run the robot again.
- Repeat until you are driving straight.
[font=Verdana]These corrective values will be different for driving forwards and backwards, so add an if statement for that.[/font]