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@1:a003b900b810, 2018-06-22 (annotated)

Committer:
whutsup
Date:
Fri Jun 22 16:37:21 2018 +0000
Revision:
1:a003b900b810
Parent:
0:cbbc3528eb59
Child:
2:b7d4195e0fea
ANGLE CONTROL

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 0:cbbc3528eb59 6 DigitalOut led(LED2); // LED
whutsup 0:cbbc3528eb59 7 DigitalOut enable(PB_12); // Motor enable
whutsup 0:cbbc3528eb59 8 DigitalOut p1(PB_13); // Motor dir1 up
whutsup 0:cbbc3528eb59 9 DigitalOut p2(PB_14); // Motor dir2 down
whutsup 0:cbbc3528eb59 10
whutsup 0:cbbc3528eb59 11 DigitalIn up(PC_8); // Burton Motor control
whutsup 0:cbbc3528eb59 12 DigitalIn down(PC_9);
whutsup 0:cbbc3528eb59 13
whutsup 0:cbbc3528eb59 14 AnalogIn disSensor(PA_7); // Distance Sensor
whutsup 0:cbbc3528eb59 15
whutsup 1:a003b900b810 16 Ticker timer1; // Control Angle By Using Distance Sensor
whutsup 1:a003b900b810 17 Ticker timer2; // Burton Control
whutsup 0:cbbc3528eb59 18
whutsup 0:cbbc3528eb59 19 /*------------------ ReadData Variables -------------------*/
whutsup 0:cbbc3528eb59 20
whutsup 0:cbbc3528eb59 21 uint8_t rx_buf[8]; // < _ _ _ _ _ _ > 8Byte
whutsup 0:cbbc3528eb59 22 char RX_command[4] = {0,}; // _ _ _
whutsup 0:cbbc3528eb59 23 char RX_data[4] = {0,}; // _ _ _
whutsup 0:cbbc3528eb59 24 char flag_RX = 0;
whutsup 0:cbbc3528eb59 25 event_callback_t Test;
whutsup 0:cbbc3528eb59 26
whutsup 0:cbbc3528eb59 27 int targetDis;
whutsup 0:cbbc3528eb59 28
whutsup 0:cbbc3528eb59 29 /*------------------ Callback Functions -------------------*/
whutsup 0:cbbc3528eb59 30
whutsup 0:cbbc3528eb59 31 void ReadData(int events)
whutsup 0:cbbc3528eb59 32 {
whutsup 0:cbbc3528eb59 33 bc.printf("Read The Data!!\n");
whutsup 0:cbbc3528eb59 34
whutsup 0:cbbc3528eb59 35
whutsup 0:cbbc3528eb59 36 if (rx_buf[0] == '<' && rx_buf[7] == '>')
whutsup 0:cbbc3528eb59 37 {
whutsup 0:cbbc3528eb59 38 flag_RX = 1;
whutsup 0:cbbc3528eb59 39
whutsup 0:cbbc3528eb59 40 RX_command[0] = rx_buf[1];
whutsup 0:cbbc3528eb59 41 RX_command[1] = rx_buf[2];
whutsup 0:cbbc3528eb59 42 RX_command[2] = rx_buf[3];
whutsup 0:cbbc3528eb59 43 RX_data[0] = rx_buf[4];
whutsup 0:cbbc3528eb59 44 RX_data[1] = rx_buf[5];
whutsup 0:cbbc3528eb59 45 RX_data[2] = rx_buf[6];
whutsup 0:cbbc3528eb59 46 }
whutsup 0:cbbc3528eb59 47
whutsup 0:cbbc3528eb59 48
whutsup 0:cbbc3528eb59 49 }
whutsup 0:cbbc3528eb59 50
whutsup 0:cbbc3528eb59 51 void ControlLED(int mode)
whutsup 0:cbbc3528eb59 52 {
whutsup 0:cbbc3528eb59 53 bc.printf("mode = %d\n",mode);
whutsup 0:cbbc3528eb59 54
whutsup 0:cbbc3528eb59 55 switch (mode)
whutsup 0:cbbc3528eb59 56 {
whutsup 0:cbbc3528eb59 57 case 0 :
whutsup 0:cbbc3528eb59 58
whutsup 0:cbbc3528eb59 59 led = 0;
whutsup 0:cbbc3528eb59 60
whutsup 0:cbbc3528eb59 61 break;
whutsup 0:cbbc3528eb59 62
whutsup 0:cbbc3528eb59 63 case 1 :
whutsup 0:cbbc3528eb59 64 led = 1;
whutsup 0:cbbc3528eb59 65
whutsup 0:cbbc3528eb59 66 break;
whutsup 0:cbbc3528eb59 67
whutsup 0:cbbc3528eb59 68 case 2 :
whutsup 0:cbbc3528eb59 69 bc.printf("ANYBARO");
whutsup 0:cbbc3528eb59 70
whutsup 0:cbbc3528eb59 71 break;
whutsup 0:cbbc3528eb59 72
whutsup 0:cbbc3528eb59 73 }
whutsup 0:cbbc3528eb59 74
whutsup 0:cbbc3528eb59 75 }
whutsup 0:cbbc3528eb59 76
whutsup 0:cbbc3528eb59 77
whutsup 0:cbbc3528eb59 78 void MotorTest(int mode)
whutsup 0:cbbc3528eb59 79 {
whutsup 0:cbbc3528eb59 80
whutsup 0:cbbc3528eb59 81 bc.printf("mode = %d\n",mode);
whutsup 0:cbbc3528eb59 82
whutsup 0:cbbc3528eb59 83 switch (mode)
whutsup 0:cbbc3528eb59 84
whutsup 0:cbbc3528eb59 85 {
whutsup 0:cbbc3528eb59 86 case 0 : //stop
whutsup 0:cbbc3528eb59 87
whutsup 0:cbbc3528eb59 88 enable = 0;
whutsup 0:cbbc3528eb59 89
whutsup 0:cbbc3528eb59 90 break;
whutsup 0:cbbc3528eb59 91
whutsup 0:cbbc3528eb59 92 case 1 : // up
whutsup 0:cbbc3528eb59 93
whutsup 0:cbbc3528eb59 94 enable = 1;
whutsup 0:cbbc3528eb59 95 p1 = 0;
whutsup 0:cbbc3528eb59 96 p2 = 1;
whutsup 0:cbbc3528eb59 97
whutsup 0:cbbc3528eb59 98 break;
whutsup 0:cbbc3528eb59 99
whutsup 0:cbbc3528eb59 100 case 2 : // down
whutsup 0:cbbc3528eb59 101
whutsup 0:cbbc3528eb59 102 enable = 1;
whutsup 0:cbbc3528eb59 103 p1 = 1;
whutsup 0:cbbc3528eb59 104 p2 = 0;
whutsup 0:cbbc3528eb59 105
whutsup 0:cbbc3528eb59 106 break;
whutsup 0:cbbc3528eb59 107
whutsup 0:cbbc3528eb59 108 }
whutsup 1:a003b900b810 109
whutsup 0:cbbc3528eb59 110
whutsup 0:cbbc3528eb59 111 }
whutsup 0:cbbc3528eb59 112
whutsup 0:cbbc3528eb59 113 void ControlAng()
whutsup 0:cbbc3528eb59 114 {
whutsup 1:a003b900b810 115
whutsup 1:a003b900b810 116 double d1 = disSensor.read();
whutsup 1:a003b900b810 117 double d2 = 47.512*d1*d1*d1*d1-180.65*d1*d1*d1+240.5*d1*d1-144.03*d1+35.556;
whutsup 1:a003b900b810 118 double ref_d = targetDis-d2;
whutsup 1:a003b900b810 119
whutsup 1:a003b900b810 120 // bc.printf("%1.3f %1.3f \n", d1, ref_d);
whutsup 1:a003b900b810 121
whutsup 1:a003b900b810 122 if(ref_d > 0.01)
whutsup 1:a003b900b810 123 {
whutsup 1:a003b900b810 124 enable = 1;
whutsup 1:a003b900b810 125 p1 = 0;
whutsup 1:a003b900b810 126 p2 = 1;
whutsup 1:a003b900b810 127
whutsup 1:a003b900b810 128 }
whutsup 1:a003b900b810 129
whutsup 1:a003b900b810 130 else if(ref_d < -0.01)
whutsup 1:a003b900b810 131 {
whutsup 1:a003b900b810 132
whutsup 1:a003b900b810 133 enable = 1;
whutsup 1:a003b900b810 134 p1 = 1;
whutsup 1:a003b900b810 135 p2 = 0;
whutsup 1:a003b900b810 136
whutsup 1:a003b900b810 137 }
whutsup 1:a003b900b810 138
whutsup 1:a003b900b810 139 else
whutsup 1:a003b900b810 140 {
whutsup 1:a003b900b810 141
whutsup 1:a003b900b810 142 enable = 0;
whutsup 1:a003b900b810 143 timer1.detach();
whutsup 1:a003b900b810 144
whutsup 1:a003b900b810 145 }
whutsup 0:cbbc3528eb59 146 }
whutsup 0:cbbc3528eb59 147
whutsup 0:cbbc3528eb59 148 void MotorBurton()
whutsup 0:cbbc3528eb59 149 {
whutsup 1:a003b900b810 150
whutsup 0:cbbc3528eb59 151 int a = up.read();
whutsup 0:cbbc3528eb59 152 int b = down.read();
whutsup 0:cbbc3528eb59 153 int c = a*2 + b;
whutsup 0:cbbc3528eb59 154
whutsup 0:cbbc3528eb59 155 switch(c)
whutsup 0:cbbc3528eb59 156 {
whutsup 0:cbbc3528eb59 157 case 0 : //stop
whutsup 0:cbbc3528eb59 158
whutsup 0:cbbc3528eb59 159 enable = 0;
whutsup 1:a003b900b810 160 timer1.detach();
whutsup 0:cbbc3528eb59 161
whutsup 0:cbbc3528eb59 162 break;
whutsup 0:cbbc3528eb59 163
whutsup 0:cbbc3528eb59 164 case 1 : // down
whutsup 0:cbbc3528eb59 165
whutsup 0:cbbc3528eb59 166 enable = 1;
whutsup 0:cbbc3528eb59 167 p1 = 0;
whutsup 0:cbbc3528eb59 168 p2 = 1;
whutsup 0:cbbc3528eb59 169
whutsup 0:cbbc3528eb59 170 break;
whutsup 0:cbbc3528eb59 171
whutsup 0:cbbc3528eb59 172 case 2 : // up
whutsup 0:cbbc3528eb59 173
whutsup 0:cbbc3528eb59 174 enable = 1;
whutsup 0:cbbc3528eb59 175 p1 = 1;
whutsup 0:cbbc3528eb59 176 p2 = 0;
whutsup 0:cbbc3528eb59 177
whutsup 0:cbbc3528eb59 178 break;
whutsup 0:cbbc3528eb59 179
whutsup 0:cbbc3528eb59 180 case 3 : // stop
whutsup 0:cbbc3528eb59 181
whutsup 0:cbbc3528eb59 182 enable = 1;
whutsup 0:cbbc3528eb59 183 p1 = 0;
whutsup 0:cbbc3528eb59 184 p2 = 0;
whutsup 0:cbbc3528eb59 185
whutsup 0:cbbc3528eb59 186 break;
whutsup 0:cbbc3528eb59 187
whutsup 0:cbbc3528eb59 188 }
whutsup 0:cbbc3528eb59 189
whutsup 0:cbbc3528eb59 190
whutsup 0:cbbc3528eb59 191
whutsup 0:cbbc3528eb59 192 }
whutsup 0:cbbc3528eb59 193
whutsup 0:cbbc3528eb59 194
whutsup 0:cbbc3528eb59 195 int main()
whutsup 0:cbbc3528eb59 196 {
whutsup 0:cbbc3528eb59 197 bc.baud(9600);
whutsup 0:cbbc3528eb59 198 bc.printf("START 180622 Ver. \n");
whutsup 0:cbbc3528eb59 199
whutsup 0:cbbc3528eb59 200 up.mode(PullNone);
whutsup 0:cbbc3528eb59 201 down.mode(PullNone);
whutsup 0:cbbc3528eb59 202
whutsup 0:cbbc3528eb59 203 int modeLED;
whutsup 0:cbbc3528eb59 204 int modeMotor;
whutsup 0:cbbc3528eb59 205
whutsup 0:cbbc3528eb59 206 modeLED = 0;
whutsup 0:cbbc3528eb59 207 modeMotor = 0;
whutsup 0:cbbc3528eb59 208
whutsup 0:cbbc3528eb59 209 while(1)
whutsup 0:cbbc3528eb59 210 {
whutsup 0:cbbc3528eb59 211
whutsup 0:cbbc3528eb59 212 bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
whutsup 0:cbbc3528eb59 213
whutsup 0:cbbc3528eb59 214 // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //
whutsup 0:cbbc3528eb59 215
whutsup 0:cbbc3528eb59 216 // analyze the recieved packets
whutsup 0:cbbc3528eb59 217
whutsup 0:cbbc3528eb59 218 if (1 == flag_RX)
whutsup 0:cbbc3528eb59 219 {
whutsup 0:cbbc3528eb59 220 flag_RX = 0;
whutsup 0:cbbc3528eb59 221
whutsup 0:cbbc3528eb59 222 if (0 == strcmp(RX_command,"LED"))
whutsup 0:cbbc3528eb59 223 {
whutsup 0:cbbc3528eb59 224 bc.printf("LED CONTROL MODE!!");
whutsup 0:cbbc3528eb59 225 modeLED = atoi(RX_data);
whutsup 0:cbbc3528eb59 226 ControlLED(modeLED);
whutsup 0:cbbc3528eb59 227 }
whutsup 0:cbbc3528eb59 228
whutsup 0:cbbc3528eb59 229 else if (0 == strcmp(RX_command,"MOT"))
whutsup 0:cbbc3528eb59 230 {
whutsup 0:cbbc3528eb59 231
whutsup 1:a003b900b810 232 bc.printf("MOTOR TEST CONTROL MODE!!");
whutsup 0:cbbc3528eb59 233 modeMotor = atoi(RX_data);
whutsup 1:a003b900b810 234 MotorTest(modeMotor);
whutsup 1:a003b900b810 235
whutsup 1:a003b900b810 236 }
whutsup 1:a003b900b810 237
whutsup 1:a003b900b810 238 else if (0 == strcmp(RX_command,"BUR"))
whutsup 1:a003b900b810 239 {
whutsup 1:a003b900b810 240
whutsup 1:a003b900b810 241 bc.printf("MOTOR BURTON CONTROL MODE!!");
whutsup 1:a003b900b810 242 timer1.detach();
whutsup 1:a003b900b810 243 timer2.attach(&MotorBurton,0.1);
whutsup 0:cbbc3528eb59 244
whutsup 0:cbbc3528eb59 245 }
whutsup 0:cbbc3528eb59 246
whutsup 0:cbbc3528eb59 247
whutsup 0:cbbc3528eb59 248
whutsup 1:a003b900b810 249 else if (0 == strcmp(RX_command,"DIS"))
whutsup 1:a003b900b810 250 {
whutsup 1:a003b900b810 251 bc.printf("MOTOR DISTANCE CONTROL MODE!!");
whutsup 1:a003b900b810 252 timer2.detach();
whutsup 1:a003b900b810 253 targetDis = atoi(RX_data);
whutsup 1:a003b900b810 254 timer1.attach(&ControlAng,0.1);
whutsup 1:a003b900b810 255
whutsup 1:a003b900b810 256 }
whutsup 0:cbbc3528eb59 257
whutsup 0:cbbc3528eb59 258
whutsup 0:cbbc3528eb59 259 }
whutsup 0:cbbc3528eb59 260
whutsup 0:cbbc3528eb59 261 wait_ms(100);
whutsup 0:cbbc3528eb59 262
whutsup 0:cbbc3528eb59 263 }
whutsup 0:cbbc3528eb59 264
whutsup 0:cbbc3528eb59 265 }