/*---------------------------------------------------------------------------- ** ** Motion.c ** ** Controls motion of the robot - speed and direction. ** **--------------------------------------------------------------------------*/ #include "Onboard.h" #include "Hardware.h" #include "Commands.h" // Define the number of motors that have speed measurement on them #define NUM_MOTORS 4 #define NUM_CONTROLLED_MOTORS 2 // Limiting current on motors before drive level is backed off. #define MOTOR_CURRENT_THRESHOLD 0xC0 // Enumeration of the motors that have speed measurement enum { FRONT_RIGHT, FRONT_LEFT, REAR_RIGHT, REAR_LEFT }; // Types of speed control. enum { OPEN_LOOP, CLOSED_LOOP }; // These globals store the measured wheel speed. Updated by ISR. static int volatile WheelSpeed[NUM_CONTROLLED_MOTORS]; static int volatile WheelToMeasure = 0; // These control the pwm ratio for the wheels. static int GeneralAdjustment = 0x8000; // Preset at half way. static int DirectionAdjustment = 0x8000; // Preset at half way. static int PwmRatio[NUM_MOTORS]; // For motor current limiting static unsigned int CurrentLimit = 0x0100; // To reduce current a little static unsigned char MotorCurrent[NUM_MOTORS]; /*---------------------------------------------------------------------------- // // MeasureNextWheelSpeed // // Every tenth visit to the scheduler (100ms) a new wheel measaurement is // initiated. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void MeasureNextWheelSpeed(void) { // // Store this wheel speed // WheelSpeed[WheelToMeasure] = WHEEL_SPEED_COUNTER; // // Go on to next wheel // if (++WheelToMeasure == NUM_MOTORS-1) WheelToMeasure = 0; SELECT_MOTOR_CHANNEL(WheelToMeasure); WHEEL_SPEED_COUNTER = 0; } /*---------------------------------------------------------------------------- // // FrcOverflow // // ISR on compare match (100ms) // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ interrupt [FRT_OCIA] void FrcOverflow(void) { CLEAR_FRC_COMPARE_FLAG; // Go on to next wheel speed measurement MeasureNextWheelSpeed(); } /*---------------------------------------------------------------------------- // // MotionInitialise // // Initialises the pins and timers for the wheel speed measurement and control // function. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void InitialiseMotion(void) { // // Set up timer 0 to count pulses from its input pin // WHEEL_SPEED_COUNTER_INIT; WHEEL_SPEED_COUNTER = 0x0000; // // Set up the free running counter to overflow every 105ms, (Phi/32) // FREE_RUNNING_COUNTER_INIT; SELECT_MOTOR_CHANNEL(0); } /*---------------------------------------------------------------------------- // // ClosedLoopWheelSpeeds // // Checks the actual wheel speeds against the required wheel speeds, and // adjusts the PWM ratios to suit. This is the closed-loop speed control // function. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ClosedLoopWheelSpeeds(void) { int i; int ActualSpeed; int ActualDirection; int AverageSpeed; // AVERAGE SPEED // Average the wheel speeds and adjust the general PWM ratio to suit // required speed. // ActualSpeed = 0; for (i=0 ; i 0x007F) GeneralAdjustment = 0x007F; // DIRECTION // Compare the ratio between the left and right wheel speeds to calculate // current turning rate. This is compared with the required turning rate // received via the radio, and the PWM ratios to the wheels adjusted // accordingly. // ActualDirection = WheelSpeed[FRONT_RIGHT] - WheelSpeed[FRONT_LEFT]; DirectionAdjustment += (Direction - ActualDirection); // Limit to within an 8-bit quantity if (DirectionAdjustment < 0xFF80) DirectionAdjustment = 0xFF80; if (DirectionAdjustment > 0x007F) DirectionAdjustment = 0x007F; // // Set the PWM ratio for each wheel. // PwmRatio[FRONT_LEFT] = CurrentLimit * (GeneralAdjustment - DirectionAdjustment) / 0x100; PwmRatio[FRONT_RIGHT] = CurrentLimit * (GeneralAdjustment + DirectionAdjustment) / 0x100; PwmRatio[REAR_LEFT] = PwmRatio[FRONT_LEFT]; PwmRatio[REAR_RIGHT] = PwmRatio[FRONT_RIGHT]; // And limit them to an 8-bit signed number for (i=0 ; i 0x007F) PwmRatio[i] = 0x007F; } } /*---------------------------------------------------------------------------- // // OpenLoopWheelSpeeds // // This is the open-loop speed control function. The PwmRatio[] settings are // simply set to the required values and no measurement is performed. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void OpenLoopWheelSpeeds(void) { int i; // Set ratios to match requests PwmRatio[FRONT_LEFT] = CurrentLimit * (Speed - Direction) / 0x100; PwmRatio[FRONT_RIGHT] = CurrentLimit * (Speed + Direction) / 0x100; PwmRatio[REAR_LEFT] = PwmRatio[FRONT_LEFT]; PwmRatio[REAR_RIGHT] = PwmRatio[FRONT_RIGHT]; // And limit them to an 8-bit signed number for (i=0 ; i 0x007F) PwmRatio[i] = 0x007F; } } /*---------------------------------------------------------------------------- // // SetPwm // // Sets the hardware PWM generators. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void SetPwm(void) { // Detect the direction if (PwmRatio[FRONT_LEFT] < 0) FRONT_LEFT_MOTOR_DIRECTION = BACKWARDS; else FRONT_LEFT_MOTOR_DIRECTION = FORWARDS; if (PwmRatio[FRONT_RIGHT] < 0) FRONT_RIGHT_MOTOR_DIRECTION = BACKWARDS; else FRONT_RIGHT_MOTOR_DIRECTION = FORWARDS; if (PwmRatio[REAR_LEFT] < 0) REAR_LEFT_MOTOR_DIRECTION = BACKWARDS; else REAR_LEFT_MOTOR_DIRECTION = FORWARDS; if (PwmRatio[REAR_RIGHT] < 0) REAR_RIGHT_MOTOR_DIRECTION = BACKWARDS; else REAR_RIGHT_MOTOR_DIRECTION = FORWARDS; // Now set the PWM ratio. FRONT_LEFT_PWM = abs((char)PwmRatio[FRONT_LEFT]) * 2; FRONT_RIGHT_PWM = abs((char)PwmRatio[FRONT_RIGHT]) * 2; REAR_LEFT_PWM = abs((char)PwmRatio[REAR_LEFT]) * 2; REAR_RIGHT_PWM = abs((char)PwmRatio[REAR_RIGHT]) * 2; } /*---------------------------------------------------------------------------- // // ProcessMotion // // Processes the motion requirements, both speed and direction. // // Modification Record: // 24-Jul-00 Paul Hills First version -----------------------------------------------------------------------------*/ void ProcessMotion(void) { char OverCurrent; int i; // // Store motor current for currently selected motor (this is the same // motor that is having its speed measured. // MotorCurrent[WheelToMeasure] = MOTOR_CURRENT; // // Check the current in each motor is not excessive. If so, back off the // PWM ratios a bit by 10%. However, if in an EMERGENCY (emergency button // on handset is presses) there is no current limiting. // OverCurrent = FALSE; for (i=0 ; i< NUM_MOTORS ; i++) if (MotorCurrent[i] > MOTOR_CURRENT_THRESHOLD) OverCurrent = TRUE; if (OverCurrent && !SpecialsRequest[EMERGENCY]) CurrentLimit = CurrentLimit * 9 / 10; else CurrentLimit = 0x100; // // Set the PWM ratios for each wheel based on requests. // if (SpecialsRequest[SPEED_CONTROL_METHOD]== CLOSED_LOOP) ClosedLoopWheelSpeeds(); else OpenLoopWheelSpeeds(); SetPwm(); }