Comment faire un Robo-Groom (3 / 3 étapes)

Étape 3: Téléchargez le code source.

 /********************************************************************* follow.c ** -------- ** runs on Create Command Module ** cover all but small opening on the front of the IR sensor ** Create will follow a virtual wall (or any IR sending out the ** force-field signal) and hopefully avoid obstacles in the process *********************************************************************/#include interrupt.h>#include io.h>#include#include "oi.h"#define TRUE 1#define FALSE 0#define FullSpeed 0x7FFF#define SlowSpeed 0x0100#define SearchSpeed 0x0100#define ExtraAngle 10#define SearchLeftAngle 125#define SearchRightAngle (SearchLeftAngle - 1000)#define CoastDistance 150#define TraceDistance 250#define TraceAngle 30#define BackDistance 25#define IRDetected (~PINB & 0x01)//states#define Ready 0#define Following 1#define WasFollowing 2#define SearchingLeft 3#define SearchingRight 4#define TracingLeft 5#define TracingRight 6#define BackingTraceLeft 7#define BackingTraceRight 8// Global variablesvolatile uint16_t timer_cnt = 0;volatile uint8_t timer_on = 0;volatile uint8_t sensors_flag = 0;volatile uint8_t sensors_index = 0;volatile uint8_t sensors_in[Sen6Size];volatile uint8_t sensors[Sen6Size];volatile int16_t distance = 0;volatile int16_t angle = 0;volatile uint8_t inRange = 0;// Functionsvoid byteTx(uint8_t value);void delayMs(uint16_t time_ms);void delayAndCheckIR(uint16_t time_ms);void delayAndUpdateSensors(unsigned int time_ms);void initialize(void);void powerOnRobot(void);void baud(uint8_t baud_code);void drive(int16_t velocity, int16_t radius);uint16_t randomAngle(void);void defineSongs(void);int main (void){//state variableuint8_t state = Ready;int found = 0;int wait_counter = 0;// Set up Create and moduleinitialize();LEDBothOff;powerOnRobot();byteTx(CmdStart);baud(Baud28800);byteTx(CmdControl);byteTx(CmdFull);// set i/o for second IR sensorDDRB &= ~0x01; //set cargo bay ePort pin 3 to be an inputPORTB |= 0x01; //set cargo ePort pin3 pullup enabled// program loopwhile(TRUE){// Stop just as a precautiondrive(0, RadStraight);// set LEDsbyteTx(CmdLeds);byteTx(((sensors[SenVWall])?LEDPlay:0x00) | (inRange?LEDAdvance:0x00));byteTx(sensors[SenCharge1]);byteTx(64);IRDetected?LED2On:LED2Off;inRange?LED1On:LED1Off;//looking for user button, check oftendelayAndUpdateSensors(10);delayAndCheckIR(10);if(UserButtonPressed) {delayAndUpdateSensors(1000);//active loopwhile(!(UserButtonPressed)&&(!sensors[SenCliffL])&&(!sensors[SenCliffFL])&&(!sensors[SenCliffFR])&&(!sensors[SenCliffR])) {byteTx(CmdLeds);byteTx(((sensors[SenVWall])?LEDPlay:0x00) | (inRange?LEDAdvance:0x00));byteTx(sensors[SenCharge1]);byteTx(255);IRDetected?LED2On:LED2Off;inRange?LED1On:LED1Off;switch(state) {case Ready:if(sensors[SenVWall]) {//check for proximity to leaderif(inRange) {drive(0,RadStraight);} else {//drive straightdrive(SlowSpeed, RadStraight);state = Following;}} else {//search for the beamangle = 0;distance = 0;wait_counter = 0;found = FALSE;drive(SearchSpeed, RadCCW);state = SearchingLeft;}break;case Following:if(sensors[SenBumpDrop] & BumpRight) {distance = 0;angle = 0;drive(-SlowSpeed, RadStraight);state=BackingTraceLeft;} else if(sensors[SenBumpDrop] & BumpLeft) {distance = 0;angle = 0;drive(-SlowSpeed, RadStraight);state=BackingTraceRight;} else if(sensors[SenVWall]) {//check for proximity to leaderif(inRange) {drive(0,RadStraight);state = Ready;} else {//drive straightdrive(FullSpeed, RadStraight);state = Following;}} else {//just lost the signal, keep going slowly one cycledistance = 0;drive(SlowSpeed, RadStraight);state = WasFollowing;}break;case WasFollowing:if(sensors[SenBumpDrop] & BumpRight) {distance = 0;angle = 0;drive(-SlowSpeed, RadStraight);state=BackingTraceLeft;} else if(sensors[SenBumpDrop] & BumpLeft) {distance = 0;angle = 0;drive(-SlowSpeed, RadStraight);state=BackingTraceRight;} else if (sensors[SenVWall]) {//check for proximity to leaderif(inRange) {drive(0,RadStraight);state = Ready;} else {//drive straightdrive(FullSpeed, RadStraight);state = Following;}} else if (distance >= CoastDistance) {drive(0,RadStraight);state = Ready;} else {drive(SlowSpeed, RadStraight);}break;case SearchingLeft:if(found) {if (angle >= ExtraAngle) {drive(SlowSpeed, RadStraight);state = Following;} else {drive(SearchSpeed, RadCCW);}} else if (sensors[SenVWall]) {found = TRUE;angle = 0;if (inRange) {drive(0,RadStraight);state = Ready;} else {drive(SearchSpeed, RadCCW);}} else if (angle >= SearchLeftAngle) {drive(SearchSpeed, RadCW);wait_counter = 0;state = SearchingRight;} else {drive(SearchSpeed, RadCCW);}break;case SearchingRight:if(found) {if (-angle >= ExtraAngle) {drive(SlowSpeed, RadStraight);state = Following;} else {drive(SearchSpeed, RadCW);}} else if (sensors[SenVWall]) {found = TRUE;angle = 0;if (inRange) {drive(0,RadStraight);state = Ready;} else {drive(SearchSpeed, RadCCW);}} else if(wait_counter > 0) {wait_counter -= 20;drive(0,RadStraight);} else if (angle = SearchRightAngle) {drive(0,RadStraight);wait_counter = 5000;angle = 0;} else {drive(SearchSpeed, RadCW);}break;case TracingLeft:if(sensors[SenBumpDrop] & BumpRight) {distance = 0;angle = 0;drive(-SlowSpeed, RadStraight);state=BackingTraceLeft;} else if(sensors[SenBumpDrop] & BumpLeft) {drive(0, RadStraight);state=Ready;} else if (sensors[SenVWall]) {//check for proximity to leaderif(inRange) {drive(0,RadStraight);state = Ready;} else {//drive straightdrive(SlowSpeed, RadStraight);state = Following;}} else if (!(distance >= TraceDistance)) {drive(SlowSpeed, RadStraight);} else if (!(-angle >= TraceAngle)) {drive(SearchSpeed, RadCW);} else {distance = 0;angle = 0;drive(SlowSpeed, RadStraight);state = Ready;}break;case TracingRight:if(sensors[SenBumpDrop] & BumpRight) {drive(0, RadStraight);state=Ready;} else if(sensors[SenBumpDrop] & BumpLeft) {distance = 0;angle = 0;drive(-SlowSpeed, RadStraight);state=BackingTraceRight;} else if (sensors[SenVWall]) {//check for proximity to leaderif(inRange) {drive(0,RadStraight);state = Ready;} else {//drive straightdrive(SlowSpeed, RadStraight);state = Following;}} else if (!(distance >= TraceDistance)) {drive(SlowSpeed, RadStraight);} else if (!(angle >= TraceAngle)) {drive(SearchSpeed, RadCCW);} else {distance = 0;angle = 0;drive(SlowSpeed, RadStraight);state = Ready;}break;case BackingTraceLeft:if (sensors[SenVWall] && inRange) {drive(0,RadStraight);state = Ready;} else if (angle >=TraceAngle) {distance = 0;angle = 0;drive(SlowSpeed, RadStraight);state = TracingLeft;} else if (-distance >= BackDistance) {drive (SearchSpeed, RadCCW);} else {drive(-SlowSpeed, RadStraight);}break;case BackingTraceRight:if (sensors[SenVWall] && inRange) {drive(0,RadStraight);state = Ready;} else if (-angle >=TraceAngle) {distance = 0;angle = 0;drive(SlowSpeed, RadStraight);state = TracingRight;} else if (-distance >= BackDistance) {drive (SearchSpeed, RadCW);} else {drive(-SlowSpeed, RadStraight);}break;default://stopdrive(0,RadStraight);state = Ready;break;}delayAndCheckIR(10);delayAndUpdateSensors(10);}//cliff or userbutton detected, allow condition to stabilize (eg, button to be released)drive(0,RadStraight);delayAndUpdateSensors(2000);}}}// Serial receive interrupt to store sensor valuesSIGNAL(SIG_USART_RECV){uint8_t temp;temp = UDR0;if(sensors_flag){sensors_in[sensors_index++] = temp;if(sensors_index >= Sen6Size)sensors_flag = 0;}}// Timer 1 interrupt to time delays in msSIGNAL(SIG_OUTPUT_COMPARE1A){if(timer_cnt)timer_cnt--;elsetimer_on = 0;}// Transmit a byte over the serial portvoid byteTx(uint8_t value){while(!(UCSR0A & _BV(UDRE0))) ;UDR0 = value;}// Delay for the specified time in ms without updating sensor valuesvoid delayMs(uint16_t time_ms){timer_on = 1;timer_cnt = time_ms;while(timer_on) ;}// Delay for the specified time in ms and check second IR detectorvoid delayAndCheckIR(uint16_t time_ms){uint8_t timer_val = 0;inRange = 0;timer_on = 1;timer_cnt = time_ms;while(timer_on) {if(!(timer_val == timer_cnt)) {inRange += IRDetected;timer_val = timer_cnt;}}inRange = (inRange>=(time_ms>>1));}// Delay for the specified time in ms and update sensor valuesvoid delayAndUpdateSensors(uint16_t time_ms){uint8_t temp;timer_on = 1;timer_cnt = time_ms;while(timer_on){if(!sensors_flag){for(temp = 0; temp Sen6Size; temp++)sensors[temp] = sensors_in[temp];// Update running totals of distance and angledistance += (int)((sensors[SenDist1] 8) | sensors[SenDist0]);angle += (int)((sensors[SenAng1] 8) | sensors[SenAng0]);byteTx(CmdSensors);byteTx(6);sensors_index = 0;sensors_flag = 1;}}}// Initialize the Mind Control's ATmega168 microcontrollervoid initialize(void){cli();// Set I/O pinsDDRB = 0x10;PORTB = 0xCF;DDRC = 0x00;PORTC = 0xFF;DDRD = 0xE6;PORTD = 0x7D;// Set up timer 1 to generate an interrupt every 1 msTCCR1A = 0x00;TCCR1B = (_BV(WGM12) | _BV(CS12));OCR1A = 71;TIMSK1 = _BV(OCIE1A);// Set up the serial port with rx interruptUBRR0 = 19;UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));// Turn on interruptssei();}void powerOnRobot(void){// If Create's power is off, turn it onif(!RobotIsOn){while(!RobotIsOn){RobotPwrToggleLow;delayMs(500); // Delay in this stateRobotPwrToggleHigh; // Low to high transition to toggle powerdelayMs(100); // Delay in this stateRobotPwrToggleLow;}delayMs(3500); // Delay for startup}}// Switch the baud rate on both Create and modulevoid baud(uint8_t baud_code){if(baud_code = 11){byteTx(CmdBaud);UCSR0A |= _BV(TXC0);byteTx(baud_code);// Wait until transmit is completewhile(!(UCSR0A & _BV(TXC0))) ;cli();// Switch the baud rate registerif(baud_code == Baud115200)UBRR0 = Ubrr115200;else if(baud_code == Baud57600)UBRR0 = Ubrr57600;else if(baud_code == Baud38400)UBRR0 = Ubrr38400;else if(baud_code == Baud28800)UBRR0 = Ubrr28800;else if(baud_code == Baud19200)UBRR0 = Ubrr19200;else if(baud_code == Baud14400)UBRR0 = Ubrr14400;else if(baud_code == Baud9600)UBRR0 = Ubrr9600;else if(baud_code == Baud4800)UBRR0 = Ubrr4800;else if(baud_code == Baud2400)UBRR0 = Ubrr2400;else if(baud_code == Baud1200)UBRR0 = Ubrr1200;else if(baud_code == Baud600)UBRR0 = Ubrr600;else if(baud_code == Baud300)UBRR0 = Ubrr300;sei();delayMs(100);}}// Send Create drive commands in terms of velocity and radiusvoid drive(int16_t velocity, int16_t radius){byteTx(CmdDrive);byteTx((uint8_t)((velocity >> 8) & 0x00FF));byteTx((uint8_t)(velocity & 0x00FF));byteTx((uint8_t)((radius >> 8) & 0x00FF));byteTx((uint8_t)(radius & 0x00FF));}