check if the final output (to be sent as PWM to motrs) is negative or positive. I only send positive PWM to my motor controller
This is clear to me, and the now included code snippet as well.
In your initial post code, there is some stuff after that, which I don't understand:
...
analogWrite(RPWM, -output);
} [color=green]// probably not good enough, but understood up to here ;)[/color]
output = -1 * output;
if(output >= 0)
{
digitalWrite(RDIRA, HIGH);
digitalWrite(RDIRB, LOW);
analogWrite(RPWM, output);
}
else
{
digitalWrite(RDIRA, LOW);
digitalWrite(RDIRB, HIGH);
analogWrite(RPWM, -output);
}
}//if(!(angle>30 || angle < -30))
else
{
analogWrite(LPWM, 0);
analogWrite(RPWM, 0);
}
I assume output should represent the speed you want your robot to move. It does not need to be exactly proportional, but you should work on that a bit, too.
- What's the minimum/maximum speed your bot can do ? For smooth and easy contol, motor should react immediately with slow speed. Max speed should rather tell you when to give up.
- What's the dynamic behavior of your balancing rod when it falls ? angle / angular velocity over time ?
- I think you don't need sin/cos even (5.7° = 0.1 * R is sufficient around your "set point") but a bit of math won't hurt.
- Did you consider that the more your sensor has left the 0° position, the more moving the motor will accelerate it ?