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;
}
