/********************************************************************** * Function: Drive_forward * @return None * @param Speed to drive both motors forward in percent, from 0 to 100. * @remark Drives both motors at the given speed. * @author David Goodman * @author Darrel Deo * @date 2013.03.27 **********************************************************************/ void Drive_forward(uint8_t speed) { desiredSpeed = speed; // Start driving the given speed setLeftMotor(speed); setRightMotor(speed); startDriveState(); }
/********************************************************************** * Function: Drive_forwardHeading * @return None * @param Speed to drive at in meters per second. * @param Heading to hold in degrees from north, from 0 to 359. * @remark Tracks the given speed and heading. * @author David Goodman * @date 2013.03.30 **********************************************************************/ void Drive_forwardHeading(uint8_t speed, uint16_t angle) { desiredSpeed = speed; desiredHeading = angle; // Start driving the given speed desiredSpeed = speed; setLeftMotor(speed); setRightMotor(speed); // Let rudder controller steer us startTrackState(); }
int main(){ //Initializations Board_init(); Serial_init(); Timer_init(); Drive_init(); ENABLE_OUT_TRIS = OUTPUT; ENABLE_OUT_LAT = MICRO; setLeftMotor(100); setRightMotor(100); while (1) asm("nop"); return SUCCESS; }
void pid( ){ // PID Regulation. //position = findMiddle( ); position = (float)interpolateMiddle( ); //*errorValue = reference-position; *errorValue = 110.5-position; if(*errorValue == 0.0){*virtualSensorValue = 0.0;} else{if(*errorValue < 0.0){*virtualSensorValue = -1.0;} else{*virtualSensorValue = 1.0;}} *integralValue = (1-iFilter)*(*integralValue) + iFilter*(*errorValue)*dt; if(*integralValue>=integralMAX){*integralValue=integralMAX;} if(*integralValue<=-integralMAX){*integralValue=-integralMAX;} *derivativeValue = (1-dFilter)*(*derivativeValue) + dFilter*(*errorValue-previousError)/dt; previousError = *errorValue; motorSpeed = (Kp*(*errorValue) + Ki*(*integralValue) + Kd*(*derivativeValue)); // Adjust the PWM. pololuLeftPWM = pololuPWM - motorSpeed; pololuRightPWM = pololuPWM + motorSpeed; *leftRegValue = (float)pololuLeftPWM; *rightRegValue = (float)pololuRightPWM; if(pololuLeftPWM>=125){ pololuLeftPWM = 125; } if(pololuRightPWM>=125){ pololuRightPWM = 125; } if(motorsIsOn){ //drive_go(motorSpeed, error); //setMotorsForwards(pololuLeftPWM,pololuRightPWM); setLeftMotor(pololuLeftPWM); setRightMotor(pololuRightPWM); } }
int main(){ //Initializations Board_init(); Serial_init(); Timer_init(); Drive_init(); ENABLE_OUT_TRIS = OUTPUT; ENABLE_OUT_LAT = MICRO; #ifdef RECIEVE_CONTROL ENABLE_OUT_LAT = RECIEVER; while(1){ ; } #endif printf("Actuator Test Harness Initiated\n\n"); //Test Rudder printf("Centering rudder.\n"); setRudder(RUDDER_TURN_LEFT, 0); delayMillisecond(ACTUATOR_DELAY); printf("Turning rudder left.\n"); setRudder(RUDDER_TURN_LEFT, 100); //push to one direction delayMillisecond(ACTUATOR_DELAY); printf("Centering rudder.\n"); setRudder(RUDDER_TURN_LEFT, 0); delayMillisecond(ACTUATOR_DELAY); printf("Turning rudder right.\n"); setRudder(RUDDER_TURN_RIGHT, 100); delayMillisecond(ACTUATOR_DELAY); printf("Centering rudder.\n"); setRudder(RUDDER_TURN_LEFT, 0); //Test Motor Left printf("Testing left motor.\n"); setLeftMotor(0); delayMillisecond(ACTUATOR_DELAY); printf("Driving left motor forward.\n"); setLeftMotor(100); delayMillisecond(ACTUATOR_DELAY); setLeftMotor(0); delayMillisecond(ACTUATOR_DELAY); //printf("Driving left motor reverse.\n"); //setLeftMotor(MIN_PULSE); //delayMillisecond(ACTUATOR_DELAY); //setLeftMotor(0); //Test Motor Right printf("Testing right motor.\n"); setRightMotor(0); delayMillisecond(ACTUATOR_DELAY); printf("Driving right motor forward.\n"); setRightMotor(100); delayMillisecond(ACTUATOR_DELAY); setRightMotor(0); delayMillisecond(ACTUATOR_DELAY); //printf("Driving right motor reverse.\n"); //setRightMotor(MIN_PULSE); //delayMillisecond(ACTUATOR_DELAY); //setRightMotor(STOP_PULSE); //delayMillisecond(ACTUATOR_DELAY); // // Remove this code // setRightMotor(MAX_PULSE); // setLeftMotor(MAX_PULSE); // while (1) // asm("nop"); // // // printf("\nDone with drive test.\n"); return SUCCESS; }
/********************************************************************** * Function: stopMotors * @return None * @remark Stops both motors from driving. **********************************************************************/ static void stopMotors() { setLeftMotor(0); setRightMotor(0); }
/** * RoverMotorRoutines: * Description: Main execution to control the motors for the rover * @param left, left motor desired speed data - derived from the movement mode * @param right, right motor desired speed data - derived from the movement mode */ void RoverMotorRoutines(motor_data* leftMotor, motor_data* rightMotor) { static unsigned long lastMilli = 0; //***OVERFLOW Condition not handled, Consider updating*** if (millis()-lastMilli >= LOOP_TIME) { if(leftMotor->targetSpeed < 0) { leftMotor->targetSpeed = abs(leftMotor->targetSpeed); if(bLeftFwd) { bLeftFwd = false; LM_count = 0; oldLMcount = 0; leftMotor->cumError = 0; leftMotor->lastError = 0; } } else if(leftMotor->targetSpeed > 0 && !bLeftFwd) { bLeftFwd = true; LM_count = 0; oldLMcount = 0; leftMotor->cumError = 0; leftMotor->lastError = 0; } if(rightMotor->targetSpeed < 0) { rightMotor->targetSpeed = abs(rightMotor->targetSpeed); if(bRightFwd) { bRightFwd = false; RM_count = 0; oldRMcount = 0; rightMotor->cumError = 0; rightMotor->lastError = 0; } } else if(rightMotor->targetSpeed > 0 && !bRightFwd) { bRightFwd = true; RM_count = 0; oldRMcount = 0; rightMotor->cumError = 0; rightMotor->lastError = 0; } lastMilli = millis(); leftMotor->measuredSpeed = calcLeftMotorSpeed(); rightMotor->measuredSpeed = calcRightMotorSpeed(); leftMotor->PWM_val = calcPWM(leftMotor); rightMotor->PWM_val = calcPWM(rightMotor); setRightMotor(bRightFwd, (rightMotor->targetSpeed!=0) * rightMotor->PWM_val); setLeftMotor(bLeftFwd, (leftMotor->targetSpeed!=0) * leftMotor->PWM_val); } }
void drive_rotate_right(int32_t m_speed){ setLeftMotor(m_speed); setRightMotor(-m_speed); }
void drive_turn_right(int32_t m_speed){ setLeftMotor(m_speed); setRightMotor(0); }
void drive_go_reverse(int32_t m_speed){ setLeftMotor(m_speed); setRightMotor(m_speed); }
void drive_go_forward(int32_t left,int32_t right){ setLeftMotor(left); setRightMotor(right); }