![](/media/cache/group/11700697_10200701668420946_8214886085951071495_o.jpg.50x50_q85.jpg)
mobile robot control with artificial hormones
Dependencies: Communication_Robot RTC-DS1307 iSerial mbed
Fork of Andante_v00 by
Diff: main.cpp
- Revision:
- 5:aae71a94cb41
- Parent:
- 4:42dc950d54f4
- Child:
- 8:af8fbf678f28
--- a/main.cpp Mon Mar 02 05:53:20 2015 +0000 +++ b/main.cpp Wed Jul 15 08:34:08 2015 +0000 @@ -7,6 +7,8 @@ #include "communication.h" #include "protocol.h" +#include "Rtc_Ds1307.h" + //control stearing PwmOut heading_front(PIN_FRONT_SERVO); @@ -27,6 +29,9 @@ COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx +Rtc_Ds1307 rtc(RTC_SDA, RTC_SCL); +Rtc_Ds1307::Time_rtc tm = {}; + void Init_servo() { heading_front.period_ms(20); @@ -36,8 +41,9 @@ void Init_foreground() { + //Update_command.attach(&getCommand,0.1); Update_command.attach(&getCommand,TIMER_UPDATE); - Samping.attach(&getSensor,TIMER_SAMPING); + //Samping.attach(&getSensor,TIMER_SAMPING); } @@ -47,24 +53,26 @@ Init_servo(); + Init_foreground(); ANDANTE_PROTOCOL_PACKET packet; + // test send datapacket packet.robotId = 0xBB; - packet.length = 8; - packet.instructionErrorId = 0x01; - packet.parameter[0] = 0x02; - packet.parameter[1] = 0x03; - packet.parameter[2] = 0x04; - packet.parameter[3] = 0x05; - packet.parameter[4] = 0x06; - packet.parameter[5] = 0x07; + packet.length = 5; + packet.instructionErrorId = 0x07; + packet.parameter[0] = 255; + packet.parameter[1] = 255; + packet.parameter[2] = 255; + pan_a.sendCommunicatePacket(&packet); - + while(1) { + // uint8_t status = pan_a.receiveCommunicatePacket(&packet); - + // pan_a.sendCommunicatePacket(&packet); + } } @@ -75,23 +83,29 @@ ANDANTE_PROTOCOL_PACKET packet; uint8_t status = pan_a.receiveCommunicatePacket(&packet); + myled=0; + + if(status == ANDANTE_ERRBIT_NONE) { - if(count >1) { + if(count >2 && count <10) { count--; } else { count=0; + } + pan_a.sendCommunicatePacket(&packet); //update command setControl(&packet); } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) { count++; - } if(count >= TIMEOUT_RESPONE_COMMAND) { //stop robot + count++; + myled=1; setSpeedControl(0.0); } } @@ -103,6 +117,7 @@ void setControl(ANDANTE_PROTOCOL_PACKET *packet) { + if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) { setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0); } @@ -125,9 +140,9 @@ return; } - if(precent <0) { + if(precent <0.0f) { us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER; - } else if(precent >0) { + } else if(precent >0.0f) { us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER; } heading_front.pulsewidth_us(us); @@ -159,9 +174,9 @@ return; } - if(precent <0) { + if(precent <0.0f) { us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER; - } else if(precent >0) { + } else if(precent >0.0f) { us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER; } drive_motor.pulsewidth_us(us);