Hur man gör en Robo-portier (3 / 3 steg)

Steg 3: Ladda ner källkoden.

/ *** follow.c **---** körs på skapa Command Module ** täcker alla men liten öppning på framsidan av IR-sensorn ** skapa kommer att följa en virtuell vägg (eller någon IR-skicka ut den ** tvingande signal) och förhoppningsvis undvika hinder i processen *** / #include interrupt.h > #include io.h > #include #include "oi.h"#define sant 1 #define falska 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 redo 0 #define följande 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; flyktiga uint8_t timer_on = 0; flyktiga uint8_t sensors_flag = 0; flyktiga uint8_t sensors_index = 0; flyktiga uint8_t sensors_in [Sen6Size], flyktiga uint8_t sensorer [Sen6Size], flyktiga int16_t distans = 0; flyktiga int16_ t vinkel = 0; flyktiga uint8_t inRange = 0; / / Functionsvoid byteTx (uint8_t värde), void delayMs (uint16_t time_ms), void delayAndCheckIR (uint16_t time_ms), void delayAndUpdateSensors (unsigned int time_ms), void initiera (void); void powerOnRobot (void); makulera baud (uint8_t baud_code), ogiltig enhet (int16_t hastighet, int16_t radie), uint16_t randomAngle (void); void defineSongs (void); int main (void) {//state variableuint8_t stat = redo; int hittade = 0; int wait_counter = 0; / / ställa upp skapa och moduleinitialize(); LEDBothOff;powerOnRobot();byteTx(CmdStart);baud(Baud28800);byteTx(CmdControl);byteTx(CmdFull); / / ange i/o för andra IR sensorDDRB & = ~ 0x01; ställa in last bay apport stift 3 vara ett inputPORTB | = 0x01; ställa in last apport pin3 pullup aktiverat / / program loopwhile(TRUE) {/ / sluta precis som en precautiondrive (0, RadStraight) ange ;// LEDsbyteTx(CmdLeds);byteTx(((sensors[SenVWall])? LEDPlay:0x00) | (inRange? LEDAdvance:0x00)), byteTx (sensorer [SenCharge1]), byteTx (64); IRDetected? LED2On:LED2Off, inRange? LED1On:LED1Off; //looking för användare knappen, 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 (sensorer [SenCharge1]), byteTx (255); IRDetected? LED2On:LED2Off; inRange? LED1On:LED1Off;switch(State) {fall Ready:if(sensors[SenVWall]) {//check för närhet till leaderif(inRange) {drive(0,RadStraight);} annat {//drive straightdrive (SlowSpeed, RadStraight), state = följande ;}} annat {//search för beamangle = 0; avstånd = 0; wait_counter = 0; hittade = FALSE; enhet (SearchSpeed, RadCCW); staten = SearchingLeft;} bryta; fall följande: if(sensors[SenBumpDrop] & BumpRight) {avstånd = 0; vinkel = 0; enhet (-SlowSpeed, RadStraight), statligt = BackingTraceLeft;} annars om (sensorer [SenBumpDrop] & BumpLeft) {avstånd = 0; vinkel = 0; enhet (-SlowSpeed, RadStraight), statligt = BackingTraceRight;} annat if(sensors[SenVWall]) {//check för närhet till leaderif(inRange) {enhet (0, RadStraight), stat = redo;} annat {//drive straightdrive (FullSpeed, RadStraight), stat = följande ;}} annat {//just förlorade signalen, hålla igång långsamt en cycledistance = 0; drive (SlowSpeed , RadStraight), statliga = WasFollowing;} bryta; WasFollowing:if(sensors[SenBumpDrop] & BumpRight) i mål {avstånd = 0; vinkel = 0; enhet (-SlowSpeed, RadStraight), statligt = BackingTraceLeft;} annars om (sensorer [SenBumpDrop] & BumpLeft) {avstånd = 0; vinkel = 0; enhet (-SlowSpeed, RadStraight), statligt = BackingTraceRight;} annars om (sensors[SenVWall]) {//check för närhet till leaderif(inRange) {enhet (0, RadStraight), stat = redo;} annat {//drive straightdrive (FullSpeed, RadStraight), stat = efter ;}} annars om (avstånd > = CoastDistance) {enhet (0 ,RadStraight), statliga = redo;} annat {enhet (SlowSpeed, RadStraight);} bryta, ärende SearchingLeft:if(found) {om (vinkel > = ExtraAngle) {enhet (SlowSpeed, RadStraight), staten = följande;} annat {enhet (SearchSpeed, RadCCW) ;}} annars om (sensors[SenVWall]) {hittade = sant; vinkel = 0; om (inRange) {enhet (0, RadStraight), staten = redo;} annat {enhet (SearchSpeed, RadCCW) ;}} annars om (vinkel > = SearchLeftAngle) {enhet (SearchSpeed, RadCW), wait_counter = 0; statligt = SearchingRight;} annat {enhet (SearchSpeed, RadCCW);} bryta, ärende SearchingRight:if(found) {om (-vinkel > = ExtraAngle) {enhet (SlowSpeed, RadStraight), staten = följande;} annat {enhet (SearchSpeed, RadCW) ;}} annars om (sensors[SenVWall]) {hittade = sant; vinkel = 0; om (inRange) {enhet (0, RadStraight), stat = redo;} annat {enhet (SearchSpeed, RadCCW) ;}} annars om (wait_counter > 0) {wait_counter-= 20;drive(0,RadStraight);} annars om (vinkel = SearchRightAngle) {enhet (0, RadStraight); wait_counter = 5000; vinkel = 0;} annat {enheten (SearchSpeed , RadCW);} bryta; TracingLeft:if(sensors[SenBumpDrop] & BumpRight) i mål {avstånd = 0; vinkel = 0; enhet (-SlowSpeed, RadStraight), statligt = BackingTraceLeft;} annars om (sensorer [SenBumpDrop] & BumpLeft) {enhet (0, RadStraight), stat = redo;} annars om (sensors[SenVWall]) {//check för närhet till leaderif(inRange) {enhet (0, RadStraight), stat = redo;} annat {//drive straightdrive (SlowSpeed, RadStraight), stat = efter ;}} annars om (! () avstånd > = TraceDistance)) {enhet (SlowSpeed, RadStraight);} else om (! () -vinkel > = TraceAngle)) {enhet (SearchSpeed, RadCW);} annat {avstånd = 0; vinkel = 0; enhet (SlowSpeed, RadStraight); state = redo;} bryta; TracingRight:if(sensors[SenBumpDrop] & BumpRight) i mål {enhet (0, RadStraight), stat = redo;} annars om (sensorer [SenBumpDrop] & BumpLeft) {avstånd = 0; vinkel = 0; enhet (-SlowSpeed, RadStraight), statligt = BackingTraceRight;} annars om (sensors[SenVWall]) {//check för närhet till leaderif(inRange) {enhet (0, RadStraight), stat = redo;} annat {//drive straightdrive (SlowSpeed, RadStraight), stat = efter ;}} annars om (! () avstånd > = TraceDistance)) {enhet (SlowSpeed, RadStraight);} else om (! () vinkel > = TraceAngle)) {enhet (SearchSpeed, RadCCW);} annat {avstånd = 0; vinkel = 0; enhet (SlowSpeed, RadStraight); state = redo;} bryta, ärende BackingTraceLeft:if (sensorer [SenVWall] & & inRange) {enhet (0, RadStraight), staten = redo;} annars om (vinkel > = TraceAngle) {avstånd = 0; vinkel = 0; enhet (SlowSpeed, RadStraight); staten = TracingLeft;} annars om (-avstånd > = BackDistance) {enhet (SearchSpeed, RadCCW);} annat {enhet (-SlowSpeed, RadStraight);} bryta, ärende BackingTraceRight:if (sensorer [SenVWall] & & inRange) {enhet (0, RadStraight), staten = redo;} annars om (-vinkel > = TraceAngle) {avstånd = 0; vinkel = 0; enhet (SlowSpeed, RadStraight); staten = TracingRight;} annars om (-avstånd > = BackDistance) {enhet (SearchSpeed, RadCW);} annat {enhet (-SlowSpeed, RadStraight);} paus, default://stopdrive (0, RadStraight), state = klar, break;} delayAndCheckIR(10);delayAndUpdateSensors(10);} klippa eller userbutton upptäckt, att villkoret att stabilisera (t.ex knappen vara released)drive(0,RadStraight); delayAndUpdateSensors(2000);}}} Följetong få avbrott att lagra sensor valuesSIGNAL(SIG_USART_RECV) {uint8_t temp, temp = UDR0;if(sensors_flag) {sensors_in [sensors_index ++] = härda; om (sensors_index > = Sen6Size) sensors_flag = 0 ;}} Timer 1-avbrott till tidsfördröjningar i msSIGNAL(SIG_OUTPUT_COMPARE1A) {om (timer_cnt) timer_cnt--; elsetimer_on = 0;} Skicka en byte via seriell portvoid byteTx (uint8_t värde) {medan)! (UCSR0A & _BV(UDRE0))) ; UDR0 = värde;} Fördröjning för den angivna tiden i ms utan att uppdatera sensor valuesvoid delayMs (uint16_t time_ms) {timer_on = 1; timer_cnt = time_ms;while(timer_on) ;} Fördröjning för den angivna tiden i ms och kolla andra IR detectorvoid delayAndCheckIR (uint16_t time_ms) {uint8_t timer_val = 0; inRange = 0; timer_on = 1; timer_cnt = time_ms;while(timer_on) {om (! () timer_val == timer_cnt)) {inRange += IRDetected; timer_val = timer_cnt ;}} inRange = (inRange > = (time_ms >> 1));} Fördröjning för den angivna tiden i ms och uppdatera sensor valuesvoid delayAndUpdateSensors (uint16_t time_ms) {uint8_t temp; timer_on = 1; timer_cnt = time_ms;while(timer_on){if(!sensors_flag) {för (temp = 0; temp Sen6Size; temp ++) sensorer [temp] = sensors_in [temp] ;// Update kör summorna för avstånd och angledistance += (int) ((sensorer [SenDist1] 8) | sensorer [SenDist0]); vinkel += (int) ((sensorer [SenAng1] 8) | sensorer [SenAng0]), byteTx (CmdSensors), byteTx (6); sensors_index = 0; sensors_flag = 1 ;}}} Initiera Mind Control ATmega168 microcontrollervoid initialize(void){cli(); / / ange i/o-pinsDDRB = 0x10; PORTB = 0XCF; DDRC = 0X00; PORTC = 0XFF; DDRD = 0XE6; PORTD = 0x7D; / / Ställ in timer 1 att generera ett avbrott varje 1 msTCCR1A = 0x00; TCCR1B = (_BV(WGM12) | _BV(CS12)); OCR1A = 71. TIMSK1 = _BV(OCIE1A); / / Ställ in serieporten med rx interruptUBRR0 = 19. UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0)); UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01)); / / slå på interruptssei();} void powerOnRobot(void) {/ / om skapa power är främst, vända den onif (! RobotIsOn) {tag (! RobotIsOn){RobotPwrToggleLow;delayMs(500); / / fördröjning i detta stateRobotPwrToggleHigh; / / låg till hög övergång att växla powerdelayMs(100); / / fördröjning i detta stateRobotPwrToggleLow;} delayMs(3500); Fördröjning för start}} / / växla överföringshastigheten på både skapa och modulevoid baud (uint8_t baud_code) {om (baud_code = 11){byteTx(CmdBaud); UCSR0A | = _BV(TXC0);byteTx(baud_code); / / vänta tills sändnings är completewhile (! () UCSR0A & _BV(TXC0))); cli(); / / växla baudhastighet registerif(baud_code == Baud115200) UBRR0 = Ubrr115200; annat if(baud_code == Baud57600) UBRR0 = Ubrr57600; annat if(baud_code == Baud38400) UBRR0 = Ubrr38400; annat if(baud_code == Baud28800) UBRR0 = Ubrr28800; annat if(baud_code == Baud19200) UBRR0 = Ubrr19200; annat if(baud_code == Baud14400) UBRR0 = Ubrr14400; annars if(baud_code == Baud9600) UBRR0 = Ubrr9600; annars if(baud_code == Baud4800) UBRR0 = Ubrr4800; annars if(baud_code == Baud2400) UBRR0 = Ubrr2400; annat if(baud_code == Baud1200) UBRR0 = Ubrr1200; annars om () baud_code == Baud600) UBRR0 = Ubrr600; annars if(baud_code == Baud300) UBRR0 = Ubrr300; sei();delayMs(100);}} Skicka Create köra kommandon i form av hastighet och radiusvoid enhet (int16_t hastighet, int16_t radius){byteTx(CmdDrive);byteTx((uint8_t) ((velocity >> 8) & 0x00FF));byteTx((uint8_t) (velocity & 0x00FF));byteTx((uint8_t) ((radie >> 8) & 0x00FF));byteTx((uint8_t)(radius & 0x00FF));}