![](/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
main.cpp@9:d0454a04f433, 2017-06-07 (annotated)
- Committer:
- soulx
- Date:
- Wed Jun 07 11:12:50 2017 +0000
- Revision:
- 9:d0454a04f433
- Parent:
- 8:af8fbf678f28
test drive; +disable communication;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
soulx | 4:42dc950d54f4 | 1 | #include "main.h" |
soulx | 0:95d17607a698 | 2 | #include "mbed.h" |
soulx | 0:95d17607a698 | 3 | #include "param.h" |
soulx | 0:95d17607a698 | 4 | #include "pin_config.h" |
soulx | 0:95d17607a698 | 5 | |
soulx | 4:42dc950d54f4 | 6 | //#include "iSerial.h" |
soulx | 2:b31c4677ebc0 | 7 | #include "communication.h" |
soulx | 4:42dc950d54f4 | 8 | #include "protocol.h" |
soulx | 2:b31c4677ebc0 | 9 | |
soulx | 5:aae71a94cb41 | 10 | #include "Rtc_Ds1307.h" |
soulx | 5:aae71a94cb41 | 11 | |
soulx | 2:b31c4677ebc0 | 12 | |
soulx | 0:95d17607a698 | 13 | //control stearing |
soulx | 0:95d17607a698 | 14 | PwmOut heading_front(PIN_FRONT_SERVO); |
soulx | 0:95d17607a698 | 15 | PwmOut heading_rear(PIN_REAR_SERVO); |
soulx | 0:95d17607a698 | 16 | //control speed |
soulx | 0:95d17607a698 | 17 | PwmOut drive_motor(PIN_RC_SPEED); |
soulx | 0:95d17607a698 | 18 | |
soulx | 0:95d17607a698 | 19 | //led debugging |
soulx | 0:95d17607a698 | 20 | DigitalOut myled(LED1); |
soulx | 0:95d17607a698 | 21 | |
soulx | 0:95d17607a698 | 22 | //set foreground |
soulx | 0:95d17607a698 | 23 | Ticker Update_command; |
soulx | 0:95d17607a698 | 24 | Ticker Samping; |
soulx | 0:95d17607a698 | 25 | |
soulx | 4:42dc950d54f4 | 26 | |
soulx | 4:42dc950d54f4 | 27 | //Init communication serial 2chanal |
soulx | 4:42dc950d54f4 | 28 | COMMUNICATION pan_a(TX_PAN_A, RX_PAN_A,115200,1000,1000); // tx, rx |
soulx | 4:42dc950d54f4 | 29 | COMMUNICATION pan_b(TX_PAN_B, RX_PAN_B,115200,1000,100); // tx, rx |
soulx | 4:42dc950d54f4 | 30 | |
soulx | 4:42dc950d54f4 | 31 | |
soulx | 5:aae71a94cb41 | 32 | Rtc_Ds1307 rtc(RTC_SDA, RTC_SCL); |
soulx | 5:aae71a94cb41 | 33 | Rtc_Ds1307::Time_rtc tm = {}; |
soulx | 5:aae71a94cb41 | 34 | |
soulx | 0:95d17607a698 | 35 | void Init_servo() |
soulx | 0:95d17607a698 | 36 | { |
soulx | 0:95d17607a698 | 37 | heading_front.period_ms(20); |
soulx | 0:95d17607a698 | 38 | heading_rear.period_ms(20); |
soulx | 0:95d17607a698 | 39 | drive_motor.period_ms(20); |
soulx | 0:95d17607a698 | 40 | } |
soulx | 0:95d17607a698 | 41 | |
soulx | 4:42dc950d54f4 | 42 | void Init_foreground() |
soulx | 4:42dc950d54f4 | 43 | { |
soulx | 5:aae71a94cb41 | 44 | //Update_command.attach(&getCommand,0.1); |
soulx | 4:42dc950d54f4 | 45 | Update_command.attach(&getCommand,TIMER_UPDATE); |
soulx | 5:aae71a94cb41 | 46 | //Samping.attach(&getSensor,TIMER_SAMPING); |
soulx | 4:42dc950d54f4 | 47 | } |
soulx | 4:42dc950d54f4 | 48 | |
soulx | 4:42dc950d54f4 | 49 | |
soulx | 4:42dc950d54f4 | 50 | int main() |
soulx | 2:b31c4677ebc0 | 51 | { |
soulx | 8:af8fbf678f28 | 52 | Timer t; |
soulx | 8:af8fbf678f28 | 53 | uint8_t action_state=1; |
soulx | 8:af8fbf678f28 | 54 | uint8_t first_time=0; |
soulx | 4:42dc950d54f4 | 55 | myled=0; |
soulx | 2:b31c4677ebc0 | 56 | |
soulx | 4:42dc950d54f4 | 57 | |
soulx | 4:42dc950d54f4 | 58 | Init_servo(); |
soulx | 5:aae71a94cb41 | 59 | Init_foreground(); |
soulx | 4:42dc950d54f4 | 60 | |
soulx | 4:42dc950d54f4 | 61 | ANDANTE_PROTOCOL_PACKET packet; |
soulx | 4:42dc950d54f4 | 62 | |
soulx | 5:aae71a94cb41 | 63 | // test send datapacket |
soulx | 4:42dc950d54f4 | 64 | packet.robotId = 0xBB; |
soulx | 5:aae71a94cb41 | 65 | packet.length = 5; |
soulx | 5:aae71a94cb41 | 66 | packet.instructionErrorId = 0x07; |
soulx | 5:aae71a94cb41 | 67 | packet.parameter[0] = 255; |
soulx | 5:aae71a94cb41 | 68 | packet.parameter[1] = 255; |
soulx | 5:aae71a94cb41 | 69 | packet.parameter[2] = 255; |
soulx | 5:aae71a94cb41 | 70 | |
soulx | 4:42dc950d54f4 | 71 | |
soulx | 4:42dc950d54f4 | 72 | pan_a.sendCommunicatePacket(&packet); |
soulx | 8:af8fbf678f28 | 73 | |
soulx | 8:af8fbf678f28 | 74 | t.start(); |
soulx | 5:aae71a94cb41 | 75 | while(1) { |
soulx | 4:42dc950d54f4 | 76 | |
soulx | 5:aae71a94cb41 | 77 | // uint8_t status = pan_a.receiveCommunicatePacket(&packet); |
soulx | 4:42dc950d54f4 | 78 | |
soulx | 5:aae71a94cb41 | 79 | // pan_a.sendCommunicatePacket(&packet); |
soulx | 8:af8fbf678f28 | 80 | switch(action_state) { |
soulx | 8:af8fbf678f28 | 81 | case 1:// forward max speec |
soulx | 8:af8fbf678f28 | 82 | if(first_time == 0) { |
soulx | 8:af8fbf678f28 | 83 | |
soulx | 8:af8fbf678f28 | 84 | setFrontAngle(0.0); |
soulx | 8:af8fbf678f28 | 85 | setRearAngle(0.0); |
soulx | 8:af8fbf678f28 | 86 | setSpeedControl(1.0); |
soulx | 8:af8fbf678f28 | 87 | t.reset(); |
soulx | 8:af8fbf678f28 | 88 | first_time=1; |
soulx | 8:af8fbf678f28 | 89 | } else if(t.read() >= 1.0) { |
soulx | 8:af8fbf678f28 | 90 | setFrontAngle(0.0); |
soulx | 8:af8fbf678f28 | 91 | setRearAngle(0.0); |
soulx | 8:af8fbf678f28 | 92 | setSpeedControl(0.0); |
soulx | 8:af8fbf678f28 | 93 | |
soulx | 8:af8fbf678f28 | 94 | action_state =2; |
soulx | 8:af8fbf678f28 | 95 | first_time = 0; |
soulx | 8:af8fbf678f28 | 96 | } |
soulx | 8:af8fbf678f28 | 97 | break; |
soulx | 8:af8fbf678f28 | 98 | case 2://backward max speed |
soulx | 8:af8fbf678f28 | 99 | break; |
soulx | 8:af8fbf678f28 | 100 | } |
soulx | 8:af8fbf678f28 | 101 | |
soulx | 8:af8fbf678f28 | 102 | |
soulx | 5:aae71a94cb41 | 103 | } |
soulx | 4:42dc950d54f4 | 104 | } |
soulx | 4:42dc950d54f4 | 105 | |
soulx | 4:42dc950d54f4 | 106 | |
soulx | 4:42dc950d54f4 | 107 | void getCommand() |
soulx | 4:42dc950d54f4 | 108 | { |
soulx | 4:42dc950d54f4 | 109 | static uint8_t count =0; |
soulx | 4:42dc950d54f4 | 110 | |
soulx | 4:42dc950d54f4 | 111 | ANDANTE_PROTOCOL_PACKET packet; |
soulx | 4:42dc950d54f4 | 112 | |
soulx | 4:42dc950d54f4 | 113 | uint8_t status = pan_a.receiveCommunicatePacket(&packet); |
soulx | 5:aae71a94cb41 | 114 | myled=0; |
soulx | 8:af8fbf678f28 | 115 | |
soulx | 5:aae71a94cb41 | 116 | |
soulx | 4:42dc950d54f4 | 117 | |
soulx | 4:42dc950d54f4 | 118 | if(status == ANDANTE_ERRBIT_NONE) { |
soulx | 5:aae71a94cb41 | 119 | if(count >2 && count <10) { |
soulx | 4:42dc950d54f4 | 120 | count--; |
soulx | 4:42dc950d54f4 | 121 | } else { |
soulx | 4:42dc950d54f4 | 122 | count=0; |
soulx | 8:af8fbf678f28 | 123 | |
soulx | 4:42dc950d54f4 | 124 | } |
soulx | 5:aae71a94cb41 | 125 | pan_a.sendCommunicatePacket(&packet); |
soulx | 4:42dc950d54f4 | 126 | //update command |
soulx | 4:42dc950d54f4 | 127 | setControl(&packet); |
soulx | 4:42dc950d54f4 | 128 | |
soulx | 4:42dc950d54f4 | 129 | } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) { |
soulx | 4:42dc950d54f4 | 130 | count++; |
soulx | 4:42dc950d54f4 | 131 | } |
soulx | 4:42dc950d54f4 | 132 | |
soulx | 4:42dc950d54f4 | 133 | if(count >= TIMEOUT_RESPONE_COMMAND) { |
soulx | 4:42dc950d54f4 | 134 | //stop robot |
soulx | 5:aae71a94cb41 | 135 | count++; |
soulx | 5:aae71a94cb41 | 136 | myled=1; |
soulx | 4:42dc950d54f4 | 137 | setSpeedControl(0.0); |
soulx | 4:42dc950d54f4 | 138 | } |
soulx | 2:b31c4677ebc0 | 139 | } |
soulx | 2:b31c4677ebc0 | 140 | |
soulx | 2:b31c4677ebc0 | 141 | void getSensor() |
soulx | 2:b31c4677ebc0 | 142 | { |
soulx | 0:95d17607a698 | 143 | |
soulx | 0:95d17607a698 | 144 | } |
soulx | 0:95d17607a698 | 145 | |
soulx | 4:42dc950d54f4 | 146 | void setControl(ANDANTE_PROTOCOL_PACKET *packet) |
soulx | 0:95d17607a698 | 147 | { |
soulx | 5:aae71a94cb41 | 148 | |
soulx | 4:42dc950d54f4 | 149 | if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) { |
soulx | 4:42dc950d54f4 | 150 | setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0); |
soulx | 4:42dc950d54f4 | 151 | } |
soulx | 4:42dc950d54f4 | 152 | |
soulx | 4:42dc950d54f4 | 153 | if(packet->instructionErrorId && ANDANTE_FRONT_ANGLE == ANDANTE_FRONT_ANGLE) { |
soulx | 4:42dc950d54f4 | 154 | setFrontAngle(((int16_t)packet->parameter[1]-127)/128.0); |
soulx | 4:42dc950d54f4 | 155 | } |
soulx | 4:42dc950d54f4 | 156 | if(packet->instructionErrorId && ANDANTE_REAR_ANGLE == ANDANTE_REAR_ANGLE) { |
soulx | 4:42dc950d54f4 | 157 | setRearAngle(((int16_t)packet->parameter[2]-127)/128.0); |
soulx | 4:42dc950d54f4 | 158 | } |
soulx | 0:95d17607a698 | 159 | } |
soulx | 0:95d17607a698 | 160 | |
soulx | 0:95d17607a698 | 161 | |
soulx | 4:42dc950d54f4 | 162 | void setFrontAngle(float precent) |
soulx | 0:95d17607a698 | 163 | { |
soulx | 4:42dc950d54f4 | 164 | int us=FRONT_CENTER; |
soulx | 4:42dc950d54f4 | 165 | |
soulx | 4:42dc950d54f4 | 166 | if(precent > (float)1.00 || precent < (float)-1.00) { |
soulx | 4:42dc950d54f4 | 167 | heading_front.pulsewidth_us(us); //STOP |
soulx | 4:42dc950d54f4 | 168 | return; |
soulx | 4:42dc950d54f4 | 169 | } |
soulx | 3:12f2ee26a43f | 170 | |
soulx | 5:aae71a94cb41 | 171 | if(precent <0.0f) { |
soulx | 4:42dc950d54f4 | 172 | us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER; |
soulx | 5:aae71a94cb41 | 173 | } else if(precent >0.0f) { |
soulx | 4:42dc950d54f4 | 174 | us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER; |
soulx | 4:42dc950d54f4 | 175 | } |
soulx | 4:42dc950d54f4 | 176 | heading_front.pulsewidth_us(us); |
soulx | 4:42dc950d54f4 | 177 | } |
soulx | 4:42dc950d54f4 | 178 | |
soulx | 4:42dc950d54f4 | 179 | void setRearAngle(float precent) |
soulx | 4:42dc950d54f4 | 180 | { |
soulx | 4:42dc950d54f4 | 181 | int us=REAR_CENTER; |
soulx | 0:95d17607a698 | 182 | |
soulx | 4:42dc950d54f4 | 183 | if(precent > (float)1.0 || precent < (float)-1.0) { |
soulx | 4:42dc950d54f4 | 184 | heading_rear.pulsewidth_us(us); //STOP |
soulx | 4:42dc950d54f4 | 185 | return; |
soulx | 4:42dc950d54f4 | 186 | } |
soulx | 4:42dc950d54f4 | 187 | |
soulx | 4:42dc950d54f4 | 188 | if(precent <0) { |
soulx | 4:42dc950d54f4 | 189 | us = (REAR_CENTER-REAR_MIN)*precent +REAR_CENTER; |
soulx | 4:42dc950d54f4 | 190 | } else if(precent >0) { |
soulx | 4:42dc950d54f4 | 191 | us = (REAR_MAX-REAR_CENTER)*precent +REAR_CENTER; |
soulx | 4:42dc950d54f4 | 192 | } |
soulx | 4:42dc950d54f4 | 193 | heading_rear.pulsewidth_us(us); |
soulx | 4:42dc950d54f4 | 194 | } |
soulx | 3:12f2ee26a43f | 195 | |
soulx | 4:42dc950d54f4 | 196 | void setSpeedControl(float precent) |
soulx | 4:42dc950d54f4 | 197 | { |
soulx | 4:42dc950d54f4 | 198 | int us=RC_SPEED_CENTER; |
soulx | 3:12f2ee26a43f | 199 | |
soulx | 4:42dc950d54f4 | 200 | if(precent > (float)1.0 || precent < (float)-1.0) { |
soulx | 4:42dc950d54f4 | 201 | drive_motor.pulsewidth_us(us); //STOP |
soulx | 4:42dc950d54f4 | 202 | return; |
soulx | 4:42dc950d54f4 | 203 | } |
soulx | 3:12f2ee26a43f | 204 | |
soulx | 5:aae71a94cb41 | 205 | if(precent <0.0f) { |
soulx | 4:42dc950d54f4 | 206 | us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER; |
soulx | 5:aae71a94cb41 | 207 | } else if(precent >0.0f) { |
soulx | 4:42dc950d54f4 | 208 | us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER; |
soulx | 3:12f2ee26a43f | 209 | } |
soulx | 4:42dc950d54f4 | 210 | drive_motor.pulsewidth_us(us); |
soulx | 4:42dc950d54f4 | 211 | } |