void DCMotor::transitory_response(int velocity, int mode, int direct){
if(mode){ //The motor is starting
if(direct == FORWARDS){
for(int i=0;i<velocity;i++){
drive_forward(velocity);
delay(_transitory_delay);
}
}else{
if(direct == BACKWARDS){
for(int i=0;i<velocity;i++){
drive_backward(velocity);
delay(_transitory_delay);
}
}
}
}else{ //The motor is coming to a halt
if(direct == FORWARDS){
for(int i=velocity;i>0;i--){
drive_forward(velocity);
delay(_transitory_delay);
}
}else{
if(direct == BACKWARDS){
for(int i=velocity;i>0;i--){
drive_backward(velocity);
delay(_transitory_delay);
}
}
}
}
}
void DCMotor::new_transitory_response(int final_velocity, int initial_velocity, int direct){
/*
* Receives a final and initial velocity and slowly transits from one to the other
*
* The values of the velocity must be between 0 and 255; direct should be either 1 for FORWARD or 2 for BACKWARD (the same codes as the instructions are saved)
*/
if(direct == 1) { //driving forward
if(final_velocity > initial_velocity){ //it's speeding up
for(int i=initial_velocity;i<final_velocity;i++){
drive_forward(i);
delay(_transitory_delay);
}
} else {
for(int i=initial_velocity;i>final_velocity;i--){
drive_forward(i);
delay(_transitory_delay);
}
}
} else { //driving backward
if(final_velocity > initial_velocity){ //it's speeding up
for(int i=initial_velocity;i<final_velocity;i++){
drive_backward(i);
delay(_transitory_delay);
}
} else {
for(int i=initial_velocity;i>final_velocity;i--){
drive_backward(i);
delay(_transitory_delay);
}
}
}
}
void loop(){
//-------------Main Program
Main:
rangeFront = readRangeFront();
rangeWall = readRangeWall();
if (rangeFront <= 400){
rangeFront = 3000;
}//endif
if(rangeWall <= 400){
rangeWall = 3000;
}//endif
// The Debugger Code. Uncomment this if you're having issues with the distance sensors.
// Make sure to comment out the main code below this debugger.
// Serial.print(rangeFront);
// Serial.print(" Front");
// Serial.println();
// delay(500);
// Serial.print(rangeWall);
// Serial.print(" Wall");
// Serial.println();
// delay(500);
//More debugging code is down there if you want to mess with it.
if (rangeFront < toCloseFront){
drive_backward();
// Serial.print(" Drive Back");
turn_right();
// Serial.print(" Right Turn");
// Serial.println();
delay(800);
drive_forward();
turn_left();
delay(1000);
goto Main;
}//endif
if(rangeWall > toCloseWall && rangeWall < toFarWall){
drive_forward();
no_turn();
// Serial.print(" Drive Forward");
// Serial.println();
goto Main;
}//endeif
if (rangeWall < toCloseWall){
turn_left();
// Serial.print(" Turn Left");
drive_forward();
// Serial.print(" Drive Forward");
// Serial.println();
goto Main;
}//endif
if (rangeWall > toFarWall){
turn_right();
// Serial.print(" Turn Right");
drive_forward();
// Serial.print(" Drive Forward");
// Serial.println();
goto Main;
}//endif
}//endVoid
