DECS @UNIST / Mbed 2 deprecated Anybaro_ver_7

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@3:b79aa7d836fb, 2018-07-20 (annotated)

Committer:
whutsup
Date:
Fri Jul 20 02:50:24 2018 +0000
Revision:
3:b79aa7d836fb
Parent:
2:b7d4195e0fea
Child:
4:bf4ad2079096
temporal save

Who changed what in which revision?

UserRevisionLine numberNew contents of line
whutsup 0:cbbc3528eb59 1 #include "mbed.h"
whutsup 0:cbbc3528eb59 2
whutsup 0:cbbc3528eb59 3 Serial bc(PA_9,PA_10); // USART1 Bluetooth (Tx, Rx)
whutsup 0:cbbc3528eb59 4 Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
whutsup 0:cbbc3528eb59 5
whutsup 3:b79aa7d836fb 6 InterruptIn up(PC_8); // Button Interrupt Motor control
whutsup 2:b7d4195e0fea 7 InterruptIn down(PC_9);
whutsup 2:b7d4195e0fea 8
whutsup 2:b7d4195e0fea 9
whutsup 0:cbbc3528eb59 10 DigitalOut led(LED2); // LED
whutsup 0:cbbc3528eb59 11 DigitalOut enable(PB_12); // Motor enable
whutsup 0:cbbc3528eb59 12 DigitalOut p1(PB_13); // Motor dir1 up
whutsup 0:cbbc3528eb59 13 DigitalOut p2(PB_14); // Motor dir2 down
whutsup 0:cbbc3528eb59 14
whutsup 3:b79aa7d836fb 15 AnalogIn disSensor(PA_7); // Distance Sensor
whutsup 0:cbbc3528eb59 16
whutsup 3:b79aa7d836fb 17 AnalogIn force_L1(PB_0); // Force Sensor
whutsup 3:b79aa7d836fb 18 AnalogIn force_L2(PB_1);
whutsup 3:b79aa7d836fb 19 AnalogIn force_L3(PC_0); // Force Sensor
whutsup 3:b79aa7d836fb 20 AnalogIn force_L4(PC_1);
whutsup 3:b79aa7d836fb 21 AnalogIn force_R1(PC_2); // Force Sensor
whutsup 3:b79aa7d836fb 22 AnalogIn force_R2(PC_3);
whutsup 3:b79aa7d836fb 23 AnalogIn force_R3(PC_4); // Force Sensor
whutsup 3:b79aa7d836fb 24 AnalogIn force_R4(PC_5);
whutsup 2:b7d4195e0fea 25
whutsup 0:cbbc3528eb59 26
whutsup 3:b79aa7d836fb 27 Ticker timer0; // Operating MCU LED on
whutsup 1:a003b900b810 28 Ticker timer1; // Control Angle By Using Distance Sensor
whutsup 2:b7d4195e0fea 29 Ticker timer2; // Force Sensor
whutsup 0:cbbc3528eb59 30
whutsup 0:cbbc3528eb59 31 /*------------------ ReadData Variables -------------------*/
whutsup 0:cbbc3528eb59 32
whutsup 0:cbbc3528eb59 33 uint8_t rx_buf[8]; // < _ _ _ _ _ _ > 8Byte
whutsup 0:cbbc3528eb59 34 char RX_command[4] = {0,}; // _ _ _
whutsup 0:cbbc3528eb59 35 char RX_data[4] = {0,}; // _ _ _
whutsup 0:cbbc3528eb59 36 char flag_RX = 0;
whutsup 0:cbbc3528eb59 37 event_callback_t Test;
whutsup 0:cbbc3528eb59 38
whutsup 0:cbbc3528eb59 39 int targetDis;
whutsup 0:cbbc3528eb59 40
whutsup 0:cbbc3528eb59 41 /*------------------ Callback Functions -------------------*/
whutsup 0:cbbc3528eb59 42
whutsup 0:cbbc3528eb59 43 void ReadData(int events)
whutsup 0:cbbc3528eb59 44 {
whutsup 0:cbbc3528eb59 45 bc.printf("Read The Data!!\n");
whutsup 0:cbbc3528eb59 46
whutsup 0:cbbc3528eb59 47
whutsup 0:cbbc3528eb59 48 if (rx_buf[0] == '<' && rx_buf[7] == '>')
whutsup 0:cbbc3528eb59 49 {
whutsup 0:cbbc3528eb59 50 flag_RX = 1;
whutsup 0:cbbc3528eb59 51
whutsup 0:cbbc3528eb59 52 RX_command[0] = rx_buf[1];
whutsup 0:cbbc3528eb59 53 RX_command[1] = rx_buf[2];
whutsup 0:cbbc3528eb59 54 RX_command[2] = rx_buf[3];
whutsup 0:cbbc3528eb59 55 RX_data[0] = rx_buf[4];
whutsup 0:cbbc3528eb59 56 RX_data[1] = rx_buf[5];
whutsup 0:cbbc3528eb59 57 RX_data[2] = rx_buf[6];
whutsup 0:cbbc3528eb59 58 }
whutsup 0:cbbc3528eb59 59
whutsup 0:cbbc3528eb59 60
whutsup 0:cbbc3528eb59 61 }
whutsup 0:cbbc3528eb59 62
whutsup 0:cbbc3528eb59 63 void ControlLED(int mode)
whutsup 0:cbbc3528eb59 64 {
whutsup 0:cbbc3528eb59 65 bc.printf("mode = %d\n",mode);
whutsup 0:cbbc3528eb59 66
whutsup 0:cbbc3528eb59 67 switch (mode)
whutsup 0:cbbc3528eb59 68 {
whutsup 0:cbbc3528eb59 69 case 0 :
whutsup 0:cbbc3528eb59 70
whutsup 0:cbbc3528eb59 71 led = 0;
whutsup 0:cbbc3528eb59 72
whutsup 0:cbbc3528eb59 73 break;
whutsup 0:cbbc3528eb59 74
whutsup 0:cbbc3528eb59 75 case 1 :
whutsup 0:cbbc3528eb59 76 led = 1;
whutsup 0:cbbc3528eb59 77
whutsup 0:cbbc3528eb59 78 break;
whutsup 0:cbbc3528eb59 79
whutsup 0:cbbc3528eb59 80 case 2 :
whutsup 0:cbbc3528eb59 81 bc.printf("ANYBARO");
whutsup 0:cbbc3528eb59 82
whutsup 0:cbbc3528eb59 83 break;
whutsup 0:cbbc3528eb59 84
whutsup 0:cbbc3528eb59 85 }
whutsup 0:cbbc3528eb59 86
whutsup 0:cbbc3528eb59 87 }
whutsup 0:cbbc3528eb59 88
whutsup 0:cbbc3528eb59 89
whutsup 0:cbbc3528eb59 90 void MotorTest(int mode)
whutsup 0:cbbc3528eb59 91 {
whutsup 0:cbbc3528eb59 92
whutsup 0:cbbc3528eb59 93 bc.printf("mode = %d\n",mode);
whutsup 0:cbbc3528eb59 94
whutsup 0:cbbc3528eb59 95 switch (mode)
whutsup 0:cbbc3528eb59 96
whutsup 0:cbbc3528eb59 97 {
whutsup 0:cbbc3528eb59 98 case 0 : //stop
whutsup 0:cbbc3528eb59 99
whutsup 0:cbbc3528eb59 100 enable = 0;
whutsup 0:cbbc3528eb59 101
whutsup 0:cbbc3528eb59 102 break;
whutsup 0:cbbc3528eb59 103
whutsup 0:cbbc3528eb59 104 case 1 : // up
whutsup 0:cbbc3528eb59 105
whutsup 0:cbbc3528eb59 106 enable = 1;
whutsup 0:cbbc3528eb59 107 p1 = 0;
whutsup 0:cbbc3528eb59 108 p2 = 1;
whutsup 0:cbbc3528eb59 109
whutsup 0:cbbc3528eb59 110 break;
whutsup 0:cbbc3528eb59 111
whutsup 0:cbbc3528eb59 112 case 2 : // down
whutsup 0:cbbc3528eb59 113
whutsup 0:cbbc3528eb59 114 enable = 1;
whutsup 0:cbbc3528eb59 115 p1 = 1;
whutsup 0:cbbc3528eb59 116 p2 = 0;
whutsup 0:cbbc3528eb59 117
whutsup 0:cbbc3528eb59 118 break;
whutsup 0:cbbc3528eb59 119
whutsup 0:cbbc3528eb59 120 }
whutsup 1:a003b900b810 121
whutsup 0:cbbc3528eb59 122
whutsup 0:cbbc3528eb59 123 }
whutsup 0:cbbc3528eb59 124
whutsup 0:cbbc3528eb59 125 void ControlAng()
whutsup 0:cbbc3528eb59 126 {
whutsup 1:a003b900b810 127
whutsup 3:b79aa7d836fb 128 float d1 = disSensor.read();
whutsup 3:b79aa7d836fb 129 float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1262.7f*d1+340.25f;
whutsup 3:b79aa7d836fb 130 // float d2 = -840.53f*d1*d1*d1+1742.1f*d1*d1-1600.0f*d1+340.25f;
whutsup 2:b7d4195e0fea 131 float ref_d = targetDis-d2;
whutsup 1:a003b900b810 132
whutsup 3:b79aa7d836fb 133 bc.printf("%1.3f %1.3f %1.3f\n", d1, d2, ref_d);
whutsup 1:a003b900b810 134
whutsup 3:b79aa7d836fb 135 if(ref_d > 0.9f)
whutsup 1:a003b900b810 136 {
whutsup 1:a003b900b810 137 enable = 1;
whutsup 1:a003b900b810 138 p1 = 0;
whutsup 1:a003b900b810 139 p2 = 1;
whutsup 1:a003b900b810 140
whutsup 1:a003b900b810 141 }
whutsup 1:a003b900b810 142
whutsup 3:b79aa7d836fb 143 else if(ref_d < -0.9f)
whutsup 1:a003b900b810 144 {
whutsup 1:a003b900b810 145
whutsup 1:a003b900b810 146 enable = 1;
whutsup 1:a003b900b810 147 p1 = 1;
whutsup 1:a003b900b810 148 p2 = 0;
whutsup 1:a003b900b810 149
whutsup 1:a003b900b810 150 }
whutsup 1:a003b900b810 151
whutsup 1:a003b900b810 152 else
whutsup 1:a003b900b810 153 {
whutsup 1:a003b900b810 154
whutsup 1:a003b900b810 155 enable = 0;
whutsup 1:a003b900b810 156 timer1.detach();
whutsup 1:a003b900b810 157
whutsup 1:a003b900b810 158 }
whutsup 0:cbbc3528eb59 159 }
whutsup 0:cbbc3528eb59 160
whutsup 3:b79aa7d836fb 161 void MotorButton()
whutsup 0:cbbc3528eb59 162 {
whutsup 1:a003b900b810 163
whutsup 0:cbbc3528eb59 164 int a = up.read();
whutsup 0:cbbc3528eb59 165 int b = down.read();
whutsup 0:cbbc3528eb59 166 int c = a*2 + b;
whutsup 0:cbbc3528eb59 167
whutsup 0:cbbc3528eb59 168 switch(c)
whutsup 0:cbbc3528eb59 169 {
whutsup 0:cbbc3528eb59 170 case 0 : //stop
whutsup 0:cbbc3528eb59 171
whutsup 0:cbbc3528eb59 172 enable = 0;
whutsup 0:cbbc3528eb59 173
whutsup 0:cbbc3528eb59 174 break;
whutsup 0:cbbc3528eb59 175
whutsup 0:cbbc3528eb59 176 case 1 : // down
whutsup 0:cbbc3528eb59 177
whutsup 0:cbbc3528eb59 178 enable = 1;
whutsup 0:cbbc3528eb59 179 p1 = 0;
whutsup 0:cbbc3528eb59 180 p2 = 1;
whutsup 0:cbbc3528eb59 181
whutsup 0:cbbc3528eb59 182 break;
whutsup 0:cbbc3528eb59 183
whutsup 0:cbbc3528eb59 184 case 2 : // up
whutsup 0:cbbc3528eb59 185
whutsup 0:cbbc3528eb59 186 enable = 1;
whutsup 0:cbbc3528eb59 187 p1 = 1;
whutsup 0:cbbc3528eb59 188 p2 = 0;
whutsup 0:cbbc3528eb59 189
whutsup 0:cbbc3528eb59 190 break;
whutsup 0:cbbc3528eb59 191
whutsup 0:cbbc3528eb59 192 case 3 : // stop
whutsup 0:cbbc3528eb59 193
whutsup 0:cbbc3528eb59 194 enable = 1;
whutsup 0:cbbc3528eb59 195 p1 = 0;
whutsup 0:cbbc3528eb59 196 p2 = 0;
whutsup 0:cbbc3528eb59 197
whutsup 0:cbbc3528eb59 198 break;
whutsup 0:cbbc3528eb59 199
whutsup 0:cbbc3528eb59 200 }
whutsup 0:cbbc3528eb59 201
whutsup 0:cbbc3528eb59 202
whutsup 0:cbbc3528eb59 203
whutsup 0:cbbc3528eb59 204 }
whutsup 0:cbbc3528eb59 205
whutsup 2:b7d4195e0fea 206 void ReadForce()
whutsup 2:b7d4195e0fea 207 {
whutsup 3:b79aa7d836fb 208 float force_L[4];
whutsup 3:b79aa7d836fb 209 float force_R[4];
whutsup 2:b7d4195e0fea 210
whutsup 3:b79aa7d836fb 211 force_L[0] = force_L1.read();
whutsup 3:b79aa7d836fb 212 force_L[1] = force_L2.read();
whutsup 3:b79aa7d836fb 213 force_L[2] = force_L3.read();
whutsup 3:b79aa7d836fb 214 force_L[3] = force_L4.read();
whutsup 3:b79aa7d836fb 215 force_R[0] = force_R1.read();
whutsup 3:b79aa7d836fb 216 force_R[1] = force_R2.read();
whutsup 3:b79aa7d836fb 217 force_R[2] = force_R3.read();
whutsup 3:b79aa7d836fb 218 force_R[3] = force_R4.read();
whutsup 3:b79aa7d836fb 219
whutsup 3:b79aa7d836fb 220 // bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_L[0],force_L[1],force_L[2],force_L[3]);
whutsup 3:b79aa7d836fb 221 // bc.printf("%1.3f, %1.3f, %1.3f, %1.3f\n", force_R[0],force_R[1],force_R[2],force_R[3]);
whutsup 2:b7d4195e0fea 222 }
whutsup 2:b7d4195e0fea 223
whutsup 3:b79aa7d836fb 224 void OperateLED()
whutsup 3:b79aa7d836fb 225 {
whutsup 3:b79aa7d836fb 226 led =!led;
whutsup 3:b79aa7d836fb 227 }
whutsup 0:cbbc3528eb59 228
whutsup 0:cbbc3528eb59 229 int main()
whutsup 0:cbbc3528eb59 230 {
whutsup 0:cbbc3528eb59 231 bc.baud(9600);
whutsup 0:cbbc3528eb59 232 bc.printf("START 180622 Ver. \n");
whutsup 3:b79aa7d836fb 233 led = 1;
whutsup 0:cbbc3528eb59 234
whutsup 0:cbbc3528eb59 235 up.mode(PullNone);
whutsup 0:cbbc3528eb59 236 down.mode(PullNone);
whutsup 0:cbbc3528eb59 237
whutsup 3:b79aa7d836fb 238 up.fall(&MotorButton); // 1 > 0 swtich on
whutsup 3:b79aa7d836fb 239 up.rise(&MotorButton); // 0 > 1 switch on
whutsup 3:b79aa7d836fb 240 down.fall(&MotorButton);
whutsup 3:b79aa7d836fb 241 down.rise(&MotorButton);
whutsup 2:b7d4195e0fea 242
whutsup 0:cbbc3528eb59 243 int modeLED;
whutsup 0:cbbc3528eb59 244 int modeMotor;
whutsup 0:cbbc3528eb59 245
whutsup 0:cbbc3528eb59 246 modeLED = 0;
whutsup 0:cbbc3528eb59 247 modeMotor = 0;
whutsup 3:b79aa7d836fb 248
whutsup 3:b79aa7d836fb 249 timer0.attach(&OperateLED,0.5);
whutsup 2:b7d4195e0fea 250 timer2.attach(&ReadForce,0.05);
whutsup 0:cbbc3528eb59 251
whutsup 3:b79aa7d836fb 252
whutsup 3:b79aa7d836fb 253
whutsup 0:cbbc3528eb59 254 while(1)
whutsup 0:cbbc3528eb59 255 {
whutsup 2:b7d4195e0fea 256
whutsup 0:cbbc3528eb59 257 bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
whutsup 0:cbbc3528eb59 258
whutsup 0:cbbc3528eb59 259 // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //
whutsup 0:cbbc3528eb59 260
whutsup 0:cbbc3528eb59 261 // analyze the recieved packets
whutsup 0:cbbc3528eb59 262
whutsup 0:cbbc3528eb59 263 if (1 == flag_RX)
whutsup 0:cbbc3528eb59 264 {
whutsup 0:cbbc3528eb59 265 flag_RX = 0;
whutsup 0:cbbc3528eb59 266
whutsup 0:cbbc3528eb59 267 if (0 == strcmp(RX_command,"LED"))
whutsup 0:cbbc3528eb59 268 {
whutsup 0:cbbc3528eb59 269 bc.printf("LED CONTROL MODE!!");
whutsup 0:cbbc3528eb59 270 modeLED = atoi(RX_data);
whutsup 0:cbbc3528eb59 271 ControlLED(modeLED);
whutsup 0:cbbc3528eb59 272 }
whutsup 0:cbbc3528eb59 273
whutsup 0:cbbc3528eb59 274 else if (0 == strcmp(RX_command,"MOT"))
whutsup 0:cbbc3528eb59 275 {
whutsup 0:cbbc3528eb59 276
whutsup 1:a003b900b810 277 bc.printf("MOTOR TEST CONTROL MODE!!");
whutsup 0:cbbc3528eb59 278 modeMotor = atoi(RX_data);
whutsup 1:a003b900b810 279 MotorTest(modeMotor);
whutsup 1:a003b900b810 280
whutsup 1:a003b900b810 281 }
whutsup 1:a003b900b810 282
whutsup 2:b7d4195e0fea 283 else if (0 == strcmp(RX_command,"POS"))
whutsup 1:a003b900b810 284 {
whutsup 2:b7d4195e0fea 285 bc.printf("MOTOR DISTANCE CONTROL MODE!!");
whutsup 1:a003b900b810 286 targetDis = atoi(RX_data);
whutsup 1:a003b900b810 287 timer1.attach(&ControlAng,0.1);
whutsup 1:a003b900b810 288
whutsup 1:a003b900b810 289 }
whutsup 0:cbbc3528eb59 290
whutsup 0:cbbc3528eb59 291
whutsup 0:cbbc3528eb59 292 }
whutsup 2:b7d4195e0fea 293
whutsup 0:cbbc3528eb59 294
whutsup 0:cbbc3528eb59 295 }
whutsup 0:cbbc3528eb59 296
whutsup 0:cbbc3528eb59 297 }