// main speed control function called by TPM2 timer overflow ISR
void ControlSpeed(void)
{
//int diff;
word tmpLeft, tmpRight;
/*
diff = (int)diffLeft - (int)diffRight;
if (diff != 0) {
// adjust the speeds of left and right motors based on the difference
//diff = diff / scaleFactor;
diff /= 65535;
tmpLeft = pwLeft - (word)diff;
tmpRight = pwRight + (word)diff;
*/
/*
if (diffLeft > diffRight) {
diff = ((int)(diffLeft - diffRight));
tmpLeft -= 2;
tmpRight += 2;
}
else {
diff = ((int)(diffRight - diffLeft));
tmpLeft = pwLeft + (word)diff;
tmpRight = pwRight - (word)diff;
}
*/
if (diffLeft > diffRight) {
tmpLeft = pwLeft - 2;
tmpRight = pwRight + 2;
}
if (diffLeft < diffRight) {
tmpLeft = pwLeft + 2;
tmpRight = pwRight - 2;
}
/*
// if the new value is greater than pwMax or smaller than pwMin, recover the original value from TPM1 channel value register;
// otherwise, update the rigster with the new value.
if (tmpLeft >= pwMin && tmpLeft <= pwMax) {
pwLeft = tmpLeft;
}
if (tmpRight >= pwMin && tmpRight <= pwMax) {
pwRight = tmpRight;
}
*/
if (tmpLeft >= pwMax) {
pwLeft = pwMax;
}
else if (tmpLeft <= pwMin)
{
pwLeft = pwMin;
}
else
{
pwLeft = tmpLeft;
}
if (tmpRight >= pwMax) {
pwRight = pwMax;
}
else if (tmpRight <= pwMin)
{
pwRight = pwMin;
}
else
{
pwRight = tmpRight;
}
// finally call ControlMotor() to reflect the changed values of pwLeft and pwRight in PWM.
if (leftMotor == MOTOR_STATUS_FORWARD)
{
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_FORWARD);
}
else if (leftMotor == MOTOR_STATUS_REVERSE)
{
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_REVERSE);
}
if (rightMotor == MOTOR_STATUS_FORWARD)
{
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_FORWARD);
}
else if (rightMotor == MOTOR_STATUS_REVERSE)
{
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_REVERSE);
}
}
void ControlMouse(MouseAction action)
{
switch (action) {
case MOUSE_ACTION_FORWARD:
if (leftMotor != MOTOR_STATUS_FORWARD) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_FORWARD);
}
if (rightMotor != MOTOR_STATUS_FORWARD) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_FORWARD);
}
if (mouseStatus != MOUSE_STATUS_FORWARD) {
mouseStatus = MOUSE_STATUS_FORWARD;
}
break;
case MOUSE_ACTION_REVERSE:
if (leftMotor != MOTOR_STATUS_REVERSE) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_REVERSE);
}
if (rightMotor != MOTOR_STATUS_REVERSE) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_REVERSE);
}
if (mouseStatus != MOUSE_STATUS_REVERSE) {
mouseStatus = MOUSE_STATUS_REVERSE;
}
break;
case MOUSE_ACTION_BRAKE:
if (leftMotor != MOTOR_STATUS_BRAKE) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_BRAKE);
}
if (rightMotor != MOTOR_STATUS_BRAKE) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_BRAKE);
}
if (mouseStatus != MOUSE_STATUS_BRAKE) {
mouseStatus = MOUSE_STATUS_BRAKE;
}
break;
case MOUSE_ACTION_STOP:
if (leftMotor != MOTOR_STATUS_STOP) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_STOP);
}
if (rightMotor != MOTOR_STATUS_STOP) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_STOP);
}
if (mouseStatus != MOUSE_STATUS_STOP) {
mouseStatus = MOUSE_STATUS_STOP;
}
break;
case MOUSE_ACTION_TURNLEFT:
if (leftMotor != MOTOR_STATUS_STOP) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_STOP);
}
if (rightMotor != MOTOR_STATUS_FORWARD) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_FORWARD);
}
if (mouseStatus != MOUSE_STATUS_TURNLEFT) {
mouseStatus = MOUSE_STATUS_TURNLEFT;
}
break;
case MOUSE_ACTION_TURNRIGHT:
if (leftMotor != MOTOR_STATUS_FORWARD) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_FORWARD);
}
if (rightMotor != MOTOR_STATUS_STOP) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_STOP);
}
if (mouseStatus != MOUSE_STATUS_TURNRIGHT) {
mouseStatus = MOUSE_STATUS_TURNRIGHT;
}
break;
case MOUSE_ACTION_TURNAROUND:
if (leftMotor != MOTOR_STATUS_FORWARD) {
ControlMotor(MOTOR_LEFT, MOTOR_ACTION_FORWARD);
}
if (rightMotor != MOTOR_STATUS_REVERSE) {
ControlMotor(MOTOR_RIGHT, MOTOR_ACTION_REVERSE);
}
if (mouseStatus != MOUSE_STATUS_TURNAROUND) {
mouseStatus = MOUSE_STATUS_TURNAROUND;
}
break;
} // end of switch()
}
