BE@R lab / Mbed 2 deprecated Andante_v000

mobile robot control with artificial hormones

Dependencies:   Communication_Robot RTC-DS1307 iSerial mbed

Fork of Andante_v00 by potiwat ngamkajornwiwat

main.cpp@5:aae71a94cb41, 2015-07-15 (annotated)

Committer:
soulx
Date:
Wed Jul 15 08:34:08 2015 +0000
Revision:
5:aae71a94cb41
Parent:
4:42dc950d54f4
Child:
8:af8fbf678f28
test

Who changed what in which revision?

UserRevisionLine numberNew 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 4:42dc950d54f4 52 myled=0;
soulx 2:b31c4677ebc0 53
soulx 4:42dc950d54f4 54
soulx 4:42dc950d54f4 55 Init_servo();
soulx 5:aae71a94cb41 56 Init_foreground();
soulx 4:42dc950d54f4 57
soulx 4:42dc950d54f4 58 ANDANTE_PROTOCOL_PACKET packet;
soulx 4:42dc950d54f4 59
soulx 5:aae71a94cb41 60 // test send datapacket
soulx 4:42dc950d54f4 61 packet.robotId = 0xBB;
soulx 5:aae71a94cb41 62 packet.length = 5;
soulx 5:aae71a94cb41 63 packet.instructionErrorId = 0x07;
soulx 5:aae71a94cb41 64 packet.parameter[0] = 255;
soulx 5:aae71a94cb41 65 packet.parameter[1] = 255;
soulx 5:aae71a94cb41 66 packet.parameter[2] = 255;
soulx 5:aae71a94cb41 67
soulx 4:42dc950d54f4 68
soulx 4:42dc950d54f4 69 pan_a.sendCommunicatePacket(&packet);
soulx 5:aae71a94cb41 70 while(1) {
soulx 4:42dc950d54f4 71
soulx 5:aae71a94cb41 72 // uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 4:42dc950d54f4 73
soulx 5:aae71a94cb41 74 // pan_a.sendCommunicatePacket(&packet);
soulx 5:aae71a94cb41 75 }
soulx 4:42dc950d54f4 76 }
soulx 4:42dc950d54f4 77
soulx 4:42dc950d54f4 78
soulx 4:42dc950d54f4 79 void getCommand()
soulx 4:42dc950d54f4 80 {
soulx 4:42dc950d54f4 81 static uint8_t count =0;
soulx 4:42dc950d54f4 82
soulx 4:42dc950d54f4 83 ANDANTE_PROTOCOL_PACKET packet;
soulx 4:42dc950d54f4 84
soulx 4:42dc950d54f4 85 uint8_t status = pan_a.receiveCommunicatePacket(&packet);
soulx 5:aae71a94cb41 86 myled=0;
soulx 5:aae71a94cb41 87
soulx 5:aae71a94cb41 88
soulx 4:42dc950d54f4 89
soulx 4:42dc950d54f4 90 if(status == ANDANTE_ERRBIT_NONE) {
soulx 5:aae71a94cb41 91 if(count >2 && count <10) {
soulx 4:42dc950d54f4 92 count--;
soulx 4:42dc950d54f4 93 } else {
soulx 4:42dc950d54f4 94 count=0;
soulx 5:aae71a94cb41 95
soulx 4:42dc950d54f4 96 }
soulx 5:aae71a94cb41 97 pan_a.sendCommunicatePacket(&packet);
soulx 4:42dc950d54f4 98 //update command
soulx 4:42dc950d54f4 99 setControl(&packet);
soulx 4:42dc950d54f4 100
soulx 4:42dc950d54f4 101 } else if(status == ANDANTE_ERRBIT_READ_TIMEOUT) {
soulx 4:42dc950d54f4 102 count++;
soulx 4:42dc950d54f4 103 }
soulx 4:42dc950d54f4 104
soulx 4:42dc950d54f4 105 if(count >= TIMEOUT_RESPONE_COMMAND) {
soulx 4:42dc950d54f4 106 //stop robot
soulx 5:aae71a94cb41 107 count++;
soulx 5:aae71a94cb41 108 myled=1;
soulx 4:42dc950d54f4 109 setSpeedControl(0.0);
soulx 4:42dc950d54f4 110 }
soulx 2:b31c4677ebc0 111 }
soulx 2:b31c4677ebc0 112
soulx 2:b31c4677ebc0 113 void getSensor()
soulx 2:b31c4677ebc0 114 {
soulx 0:95d17607a698 115
soulx 0:95d17607a698 116 }
soulx 0:95d17607a698 117
soulx 4:42dc950d54f4 118 void setControl(ANDANTE_PROTOCOL_PACKET *packet)
soulx 0:95d17607a698 119 {
soulx 5:aae71a94cb41 120
soulx 4:42dc950d54f4 121 if(packet->instructionErrorId && ANDANTE_SPEED_CONTROL == ANDANTE_SPEED_CONTROL) {
soulx 4:42dc950d54f4 122 setSpeedControl(((int16_t)packet->parameter[0]-127)/128.0);
soulx 4:42dc950d54f4 123 }
soulx 4:42dc950d54f4 124
soulx 4:42dc950d54f4 125 if(packet->instructionErrorId && ANDANTE_FRONT_ANGLE == ANDANTE_FRONT_ANGLE) {
soulx 4:42dc950d54f4 126 setFrontAngle(((int16_t)packet->parameter[1]-127)/128.0);
soulx 4:42dc950d54f4 127 }
soulx 4:42dc950d54f4 128 if(packet->instructionErrorId && ANDANTE_REAR_ANGLE == ANDANTE_REAR_ANGLE) {
soulx 4:42dc950d54f4 129 setRearAngle(((int16_t)packet->parameter[2]-127)/128.0);
soulx 4:42dc950d54f4 130 }
soulx 0:95d17607a698 131 }
soulx 0:95d17607a698 132
soulx 0:95d17607a698 133
soulx 4:42dc950d54f4 134 void setFrontAngle(float precent)
soulx 0:95d17607a698 135 {
soulx 4:42dc950d54f4 136 int us=FRONT_CENTER;
soulx 4:42dc950d54f4 137
soulx 4:42dc950d54f4 138 if(precent > (float)1.00 || precent < (float)-1.00) {
soulx 4:42dc950d54f4 139 heading_front.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 140 return;
soulx 4:42dc950d54f4 141 }
soulx 3:12f2ee26a43f 142
soulx 5:aae71a94cb41 143 if(precent <0.0f) {
soulx 4:42dc950d54f4 144 us = (FRONT_CENTER-FRONT_MIN)*precent +FRONT_CENTER;
soulx 5:aae71a94cb41 145 } else if(precent >0.0f) {
soulx 4:42dc950d54f4 146 us = (FRONT_MAX-FRONT_CENTER)*precent +FRONT_CENTER;
soulx 4:42dc950d54f4 147 }
soulx 4:42dc950d54f4 148 heading_front.pulsewidth_us(us);
soulx 4:42dc950d54f4 149 }
soulx 4:42dc950d54f4 150
soulx 4:42dc950d54f4 151 void setRearAngle(float precent)
soulx 4:42dc950d54f4 152 {
soulx 4:42dc950d54f4 153 int us=REAR_CENTER;
soulx 0:95d17607a698 154
soulx 4:42dc950d54f4 155 if(precent > (float)1.0 || precent < (float)-1.0) {
soulx 4:42dc950d54f4 156 heading_rear.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 157 return;
soulx 4:42dc950d54f4 158 }
soulx 4:42dc950d54f4 159
soulx 4:42dc950d54f4 160 if(precent <0) {
soulx 4:42dc950d54f4 161 us = (REAR_CENTER-REAR_MIN)*precent +REAR_CENTER;
soulx 4:42dc950d54f4 162 } else if(precent >0) {
soulx 4:42dc950d54f4 163 us = (REAR_MAX-REAR_CENTER)*precent +REAR_CENTER;
soulx 4:42dc950d54f4 164 }
soulx 4:42dc950d54f4 165 heading_rear.pulsewidth_us(us);
soulx 4:42dc950d54f4 166 }
soulx 3:12f2ee26a43f 167
soulx 4:42dc950d54f4 168 void setSpeedControl(float precent)
soulx 4:42dc950d54f4 169 {
soulx 4:42dc950d54f4 170 int us=RC_SPEED_CENTER;
soulx 3:12f2ee26a43f 171
soulx 4:42dc950d54f4 172 if(precent > (float)1.0 || precent < (float)-1.0) {
soulx 4:42dc950d54f4 173 drive_motor.pulsewidth_us(us); //STOP
soulx 4:42dc950d54f4 174 return;
soulx 4:42dc950d54f4 175 }
soulx 3:12f2ee26a43f 176
soulx 5:aae71a94cb41 177 if(precent <0.0f) {
soulx 4:42dc950d54f4 178 us = (RC_SPEED_CENTER-RC_SPEED_MIN)*precent +RC_SPEED_CENTER;
soulx 5:aae71a94cb41 179 } else if(precent >0.0f) {
soulx 4:42dc950d54f4 180 us = (RC_SPEED_MAX-RC_SPEED_CENTER)*precent +RC_SPEED_CENTER;
soulx 3:12f2ee26a43f 181 }
soulx 4:42dc950d54f4 182 drive_motor.pulsewidth_us(us);
soulx 4:42dc950d54f4 183 }