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 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 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
task autonomous(){ drive_forward(1000, 50); }
int main(void) { //setting PWM-Ports as output DDRB = 0xFF; //set port B as output DDRD = 0b11111101; //set port D as output with pin D1 input //setup the pwm for the servo TCCR1A = _BV(COM1A1) // set OC1A/B at TOP | _BV(COM1B1) // clear OC1A/B when match | _BV(WGM11); //(fast PWM, clear TCNT1 on match ICR1) TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // timer uses main system clock with 1/8 prescale ICR1 = 20000; // used for TOP, makes for 50 hz PWM OCR1A = 1500; // servo at center //set up pwm for the motors TCCR0A = _BV(COM0A1) // set OC1A/B at TOP | _BV(COM0B1) // clear OC1A/B when match | _BV(WGM00) // fast PWM mode | _BV(WGM01); TCCR0B = _BV(CS01) | _BV(CS00); drive_stop(0); //motors stopped servo_position(servo_center, 50); //servo center //variables to hold the distances; uint16_t forward_dist; uint16_t left_dist; uint16_t right_dist; uint8_t fwd_count; servo_position(servo_center, 50); while(1) { forward_dist = sensor_distance(); _delay_ms(50); if (forward_dist <= forward_buffer) { drive_stop(0); beep_meow(); servo_position(servo_left, 400); left_dist = sensor_distance(); servo_position(servo_right,400); right_dist = sensor_distance(); servo_position(servo_center,200); while ((left_dist > right_dist) && (forward_dist <= (forward_buffer + now_clear))) { drive_left(speed_80p); forward_dist = sensor_distance(); _delay_ms(50); } while ((left_dist <= right_dist) && (forward_dist <= (forward_buffer + now_clear))) { drive_right(speed_80p); forward_dist = sensor_distance(); _delay_ms(50); } } drive_forward(speed_80p); fwd_count++; if (fwd_count == 20) { fwd_count = 0; servo_position(servo_left, 150); left_dist = sensor_distance(); if (left_dist <= sides_buffer) { drive_stop(0); beep_meow(); while (left_dist <= (sides_buffer + now_clear)) { drive_right(speed_80p); left_dist = sensor_distance(); _delay_ms(100); } } servo_position(servo_center,100); servo_position(servo_right, 150); right_dist = sensor_distance(); if (right_dist <= sides_buffer) { drive_stop(0); beep_meow(); while (right_dist <= (sides_buffer + now_clear)) { drive_left(speed_80p); right_dist = sensor_distance(); _delay_ms(100); } } servo_position(servo_center,100); } } return 0; }