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@2:b7d4195e0fea, 2018-06-23 (annotated)

Committer:
whutsup
Date:
Sat Jun 23 09:10:07 2018 +0000
Revision:
2:b7d4195e0fea
Parent:
1:a003b900b810
Child:
3:b79aa7d836fb
force sensors test(0623)

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