#include <S12e128bits.h> #include <mc9s12e128.h> #include "Pins.h" #include <stdio.h> #include "ADS12.H" static float voltageScaler; void InitDriveMotors(void){ short inductiveReading = ADS12_ReadADPin(PIN_BACK_RIGHT_INDUCTOR); short voltageReading = ADS12_ReadADPin(PIN_VOLTAGE); voltageScaler = ((float)voltageReading)/1023; printf("Inductive val: %u \n\r", inductiveReading); printf("AD Value: %u \n\r", voltageReading); //Init Hardware // PWM - 3:right, 2:left // Enable PWME |= _S12_PWME2 | _S12_PWME3; // Pre-scaler PWMPRCLK |= DRIVE_PRESCALER; // PWM Clock Select (B) PWMCLK &= ~(_S12_PCLK2 | _S12_PCLK3); // Set the period PWMPER2 = 100; PWMPER3 = 100; // Initial duty = 0 for brake LEFT_DUTY = 0; RIGHT_DUTY = 0; // Polarity Low PWMPOL &= ~(_S12_PPOL2 | _S12_PPOL3); // Direction Lines - Outputs DDR_DRIVE_DIR |= PIN_LEFT_DIR | PIN_RIGHT_DIR; //Set initially High PORT_DRIVE_DIR |= PIN_LEFT_DIR | PIN_RIGHT_DIR; printf("Init Drive Motors \n\r"); } void DriveBackward(void){ //Direction High PORT_DRIVE_DIR |= PIN_LEFT_DIR | PIN_RIGHT_DIR; // Polarity Low PWMPOL &= ~(_S12_PPOL2 | _S12_PPOL3); //Start Driving LEFT_DUTY = LEFT_BACKWARD; RIGHT_DUTY = RIGHT_BACKWARD; } void DriveForward(void){ //Direction - Low PORT_DRIVE_DIR &= ~(PIN_LEFT_DIR | PIN_RIGHT_DIR); // Polarity High PWMPOL |= _S12_PPOL2 | _S12_PPOL3; LEFT_DUTY = LEFT_FORWARD; RIGHT_DUTY = RIGHT_FORWARD; } void TurnCCW(void){ //Left Wheel Backward PORT_DRIVE_DIR |= PIN_LEFT_DIR; PWMPOL &= ~_S12_PPOL3; //Right Wheel Forward PORT_DRIVE_DIR &= ~PIN_RIGHT_DIR; PWMPOL |= _S12_PPOL2; RIGHT_DUTY = TURN_SPEED; LEFT_DUTY = TURN_SPEED; } void TurnCW(void){ //Right Wheel Backward PORT_DRIVE_DIR |= PIN_RIGHT_DIR; PWMPOL &= ~_S12_PPOL2; //Left Wheel Forward PORT_DRIVE_DIR &= ~PIN_LEFT_DIR; PWMPOL |= _S12_PPOL3; RIGHT_DUTY = TURN_SPEED; LEFT_DUTY = TURN_SPEED; } void StopMotor(void){ // Polarity High PWMPOL |= _S12_PPOL2 | _S12_PPOL3; //Change Direction - HIGH PORT_DRIVE_DIR |= PIN_LEFT_DIR | PIN_RIGHT_DIR; LEFT_DUTY = 100; RIGHT_DUTY = 100; printf("Stop Motors \n\r"); } void Forward_PID(int leftDriveSpeed, int rightDriveSpeed, float Kp, float Kd){ static int ErrorSum = 0; static int LastError = 0; float PWMReduction; //Read Inductor Values short LeftVal = ADS12_ReadADPin(PIN_FRONT_LEFT_INDUCTOR); short RightVal = ADS12_ReadADPin(PIN_FRONT_RIGHT_INDUCTOR); signed int Error = LeftVal - RightVal; //Change Direction - Low PORT_DRIVE_DIR &= ~(PIN_LEFT_DIR | PIN_RIGHT_DIR); // Polarity High PWMPOL |= _S12_PPOL2 | _S12_PPOL3; //PID PWMReduction = (Kp*Error*voltageScaler + Kd*(Error-LastError)*voltageScaler ); //+ Ki*ErrorSum + Kd*(Error - LastError); if(PWMReduction > 0){ //Right at 100% RIGHT_DUTY = (char)rightDriveSpeed; //Reduce Left DC if((leftDriveSpeed - (char)(PWMReduction)) < 0){ LEFT_DUTY = 0; }else{ LEFT_DUTY = (char)((float)leftDriveSpeed - PWMReduction); } }else{ //Left at 100% LEFT_DUTY = (char)leftDriveSpeed; //Reduce Right DC if((rightDriveSpeed + (char)(PWMReduction)) < 0){ RIGHT_DUTY = 0; }else{ RIGHT_DUTY = (char)((float)rightDriveSpeed + PWMReduction); } } printf("PWMReduction: %f, Left Val: %d, Right Val: %d, Left PWM: %d, Right PWM %d \n\r", PWMReduction, LeftVal, RightVal, LEFT_DUTY, RIGHT_DUTY); LastError = Error; } void Backward_PID(int leftDriveSpeed, int rightDriveSpeed, float Kp, float Kd){ static int ErrorSum = 0; static float LastError = 0; float PWMReduction; //Read Inductor Values short LeftVal = ADS12_ReadADPin(PIN_BACK_LEFT_INDUCTOR); short RightVal = ADS12_ReadADPin(PIN_BACK_RIGHT_INDUCTOR); float Error = (float)LeftVal - (float)RightVal; //Direction High PORT_DRIVE_DIR |= PIN_LEFT_DIR | PIN_RIGHT_DIR; // Polarity Low PWMPOL &= ~(_S12_PPOL2 | _S12_PPOL3); //PID PWMReduction = (Kp*Error*voltageScaler + Kd*(Error-LastError)*voltageScaler ); //+ Ki*ErrorSum + Kd*(Error - LastError); if(PWMReduction > 0){ //Right at 100% RIGHT_DUTY = (char)rightDriveSpeed; //Reduce Left DC if((leftDriveSpeed - (char)(PWMReduction)) < 0){ LEFT_DUTY = 0; } else{ LEFT_DUTY = (char)((float)leftDriveSpeed - PWMReduction); } }else{ //Left at 100% LEFT_DUTY = (char)leftDriveSpeed; //Reduce Right DC if((rightDriveSpeed + (char)(PWMReduction)) < 0){ RIGHT_DUTY = 0; }else{ RIGHT_DUTY = (char)((float)rightDriveSpeed + PWMReduction); } } printf("Left Val: %d, Right Val: %d, PWMReduction: %f, Left PWM: %d, Right PWM %d \n\r", LeftVal, RightVal, PWMReduction, LEFT_DUTY, RIGHT_DUTY); LastError = Error; }