// 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() }