void forward(USHORT lspeed,USHORT rspeed) { lmotor.setReverse(true); rmotor.setReverse(false); lmotor.setAcceleration(normal_acc); rmotor.setAcceleration(normal_acc); lmotor.setTargetSpeed(lspeed); rmotor.setTargetSpeed(rspeed); }
//Actions void stop (USHORT acc) { int motorcounter =0; lmotor.setAcceleration(acc); rmotor.setAcceleration(acc); lmotor.setTargetSpeed(0); //stop rmotor.setTargetSpeed(0); while ( rmotor.getCurrentSpeed()>1 && motorcounter<stop_timeout) { motorcounter++; } }
void reverse(USHORT speed) //straight back { lmotor.setReverse(false); rmotor.setReverse(true); lmotor.setAcceleration(normal_acc); rmotor.setAcceleration(normal_acc); lmotor.setTargetSpeed(speed); rmotor.setTargetSpeed(speed); for(int del =0;del<20000;del++) { for(int eel =0;eel<reverse_delay;eel++) {} } }
void turn_left(bool inplace, USHORT lspeed, USHORT rspeed) { lmotor.setAcceleration(normal_acc); rmotor.setAcceleration(normal_acc); lmotor.setTargetSpeed(lspeed); //stop rmotor.setTargetSpeed(rspeed); lmotor.setReverse(!inplace); rmotor.setReverse(false); for(int del =0;del<20000;del++) { for(int eel =0;eel<turn_delay;eel++) {} } }
int main() { state = state_search; //USHORT speed =search_speed; Setup_Sumo_Ports(); line_triggered=false; ioinit (); // enable interrupts sei(); // create timer that executes refreshDotMatrix() at 4000 hertz Timer::instance(0)->setFrequency(1000); Timer::instance(0)->setFunctionPtr(refreshDotMatrix); Timer::instance(0)->start(); lmotor.setPhaseRegisters(&LEFT_PHASE_A, &LEFT_PHASE_B); rmotor.setPhaseRegisters(&RIGHT_PHASE_A, &RIGHT_PHASE_B); lmotor.setTimerNumber(1); rmotor.setTimerNumber(5); lmotor.setTargetSpeed(0); rmotor.setTargetSpeed(0); lmotor.setTorque(255); rmotor.setTorque(255); lmotor.setAcceleration(normal_acc); rmotor.setAcceleration(normal_acc); lmotor.setReverse(true); rmotor.setReverse(false); while(countdownrunning)//wait for countdown {} Timer::instance(1)->setFrequency(200); Timer::instance(1)->setFunctionPtr(leftMotorUpdate); Timer::instance(1)->start(); Timer::instance(5)->setFrequency(200); Timer::instance(5)->setFunctionPtr(rightMotorUpdate); Timer::instance(5)->start(); // enable the adc A2DConverter::enable(); IRSensor IRL(UBYTE(14)); //left IRSensor IRB(UBYTE(13)); //back IRSensor IRR(UBYTE(12)); //right IRSensor IRFR(UBYTE(11)); //front right IRSensor IRFL(UBYTE(0)); //frong left IRSensor * rangeSensors[5]; rangeSensors[0] = &IRFL; rangeSensors[1] = &IRFR; rangeSensors[2] = &IRR; rangeSensors[3] = &IRB; rangeSensors[4] = &IRL; lines.SetFunctionPtr(checkLines); if(enable_lines) lines.EnableFunction(); USHORT counter =0; //displayInt(speed); while (true) { if(counter == 2000) { double d = volts.GetVoltageBatt(); displayFloat(d,2); counter = 0; } counter++; switch (state) { case state_search: { DotMatrix::instance()->loadSprite( sc_hexDigits_mini[1], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); forward(search_speed,search_speed); USHORT fl =0; USHORT fr=0; USHORT r=0; USHORT b=0; USHORT l=0; //read IR sensors x avg times SHORT avg = 10; for(SHORT c = 0;c<avg;c++) { fl += IRFL.ReadSensorDist(); fr += IRFR.ReadSensorDist(); r += IRR.ReadSensorDist(); b += IRB.ReadSensorDist(); l += IRL.ReadSensorDist(); } fl = fl/avg; fr = fr/avg; r = r/avg; b = b/avg; l = l/avg; if(fl<IR_front_threshold||fr<IR_front_threshold) //obstacle in front { state = state_obstacle_front; } else if(r < IR_side_threshold) { state = state_obstacle_right; } else if(l < IR_side_threshold) { state = state_obstacle_left; } else if(b < IR_back_threshold) { state = state_obstacle_back; } if(lines.GetFunctionEnabled()) { lines.CheckLines(); checkLines(); } break; } case state_obstacle_front: { USHORT dot = 1; DotMatrix::instance()->loadSprite(&dot,1,1,0,0); DotMatrix::instance()->loadSprite(&dot,1,1,7,0); DotMatrix::instance()->loadSprite( sc_hexDigits_mini[2], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); USHORT fl,fr; //read IR sensors fl = IRFL.ReadSensorDist(); fr = IRFR.ReadSensorDist(); static SHORT rmotor_diff = 0; if(fl>IR_front_threshold&&fr>IR_front_threshold) //obstacle in not front { state = state_search; //go back to looking rmotor_diff = 0; //clear dots dot =0; DotMatrix::instance()->loadSprite(&dot,1,1,0,0); DotMatrix::instance()->loadSprite(&dot,1,1,7,0); } if(fl < fr) //veer left { if(rmotor_diff<100) rmotor_diff += 12; dot =0; //dot on left, clear right DotMatrix::instance()->loadSprite(&dot,1,1,0,0); dot =1; DotMatrix::instance()->loadSprite(&dot,1,1,7,0); } else //veer right { if(rmotor_diff > -100) rmotor_diff -= 12; dot =1; //dot on left, clear right DotMatrix::instance()->loadSprite(&dot,1,1,0,0); dot =0; DotMatrix::instance()->loadSprite(&dot,1,1,7,0); } forward(search_speed,search_speed + rmotor_diff); break; } case state_obstacle_right: { DotMatrix::instance()->loadSprite( sc_hexDigits_mini[3], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); turn_right(true,search_speed,search_speed); state = state_search; break; } case state_obstacle_left: { DotMatrix::instance()->loadSprite( sc_hexDigits_mini[4], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); turn_left(true,search_speed,search_speed); state = state_search; break; } case state_obstacle_back: { DotMatrix::instance()->loadSprite( sc_hexDigits_mini[5], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); turn_right(true,search_speed,search_speed); state = state_search; break; } case state_line_detected: { /* line1 = front right line2 = back right line3 = back left line4 = front left */ int timeout = 0; DotMatrix::instance()->loadSprite( sc_hexDigits_mini[6], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); if(lines.GetLineDetected(2) || lines.GetLineDetected(3)) //line at rear, go forward { state = state_search; } else //reverse and turn { stop(3000); bool line_left = true; if(lines.GetLineDetected(1)) //which side is the line on? line_left = false; reverse(search_speed); if(!line_left) //turn away from line turn_left(true,search_speed,search_speed); else turn_right(true,search_speed,search_speed); stop(3000); state = state_search; } break; } case state_rush: { state = state_search; break; } case state_push: { state = state_search; break; } case state_search_left: { state = state_search; break; } case state_search_right: { state = state_search; break; } } } /* while (true) { //USHORT adcValue = A2DConverter::getSample(14); //adcValue++; UBYTE numavg = 30; UINT adcValue = 0; for(UBYTE i=0;i< numavg;i++) { adcValue += IRB.ReadSensorAD(); } adcValue = adcValue/numavg; //average numavg points UBYTE adcIndex = UBYTE(adcValue % 10); //1s DotMatrix::instance()->loadSprite( sc_hexDigits_mini[adcIndex], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 12 ); adcIndex = UBYTE((adcValue/10) % 10); //10s DotMatrix::instance()->loadSprite( sc_hexDigits_mini[adcIndex], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 8 ); adcIndex = UBYTE((adcValue/100) % 10); //100s DotMatrix::instance()->loadSprite( sc_hexDigits_mini[adcIndex], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 4 ); adcIndex = UBYTE((adcValue/1000) % 10); //1000s DotMatrix::instance()->loadSprite( sc_hexDigits_mini[adcIndex], SPRITE_ALPHANUMERIC_MINI_WIDTH, SPRITE_ALPHANUMERIC_MINI_HEIGHT, 1, 0 ); for(int del =0;del<10000;del++) { for(int eel =0;eel<70;eel++) {} } } */ return int(0); }