/**********************************************************************
* 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);
}
