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@15:e5e34512a00e, 2019-07-04 (annotated)

Committer:
Bhoney
Date:
Thu Jul 04 07:02:38 2019 +0000
Revision:
15:e5e34512a00e
Parent:
14:d059d99e9b5e
Child:
19:67584cb64b9c
demo ver .2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
whutsup 0:cbbc3528eb59 1 #include "mbed.h"
whutsup 4:bf4ad2079096 2 #include "MotorControl.h"
whutsup 4:bf4ad2079096 3 #include "ForceRead.h"
whutsup 0:cbbc3528eb59 4
whutsup 4:bf4ad2079096 5 Serial bt(PA_9,PA_10); // USART1 Bluetooth (Tx, Rx)
Bhoney 12:75fb40f38c19 6 //Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
Bhoney 12:75fb40f38c19 7 Serial pc(USBTX,USBRX); // USART2 pc (Tx, Rx)
whutsup 2:b7d4195e0fea 8
whutsup 4:bf4ad2079096 9 InterruptIn up(PC_8); // Button Interrupt Motor control
whutsup 4:bf4ad2079096 10 InterruptIn down(PC_9);
whutsup 8:a435e7aa7a02 11 InterruptIn bt_ml(PC_10); // Millking action button
whutsup 4:bf4ad2079096 12
whutsup 9:7d6fa62f9022 13 DigitalOut led(PA_5); // Operating LED signal
whutsup 4:bf4ad2079096 14
whutsup 4:bf4ad2079096 15 AnalogIn disSensor(PA_7); // Distance Sensor
whutsup 0:cbbc3528eb59 16
whutsup 4:bf4ad2079096 17 Timeout ResetTimer; // Reset angle 0 position
whutsup 4:bf4ad2079096 18
whutsup 10:3fcaf50f528f 19 Ticker timer0;
whutsup 4:bf4ad2079096 20 Ticker timer1; // Control Angle By Using Distance Sensor
whutsup 4:bf4ad2079096 21 Ticker timer2; // Force Sensor
whutsup 4:bf4ad2079096 22 Ticker timer3; // Millking Action Mode
whutsup 11:5f05b14649ee 23 //Ticker timer4;
whutsup 0:cbbc3528eb59 24
whutsup 4:bf4ad2079096 25 int targetDis; // Target Of Distance
whutsup 4:bf4ad2079096 26 int milLoop; // Number of Millking Action
whutsup 8:a435e7aa7a02 27 //float errorPrevious;
whutsup 8:a435e7aa7a02 28
whutsup 4:bf4ad2079096 29 int onewayNum=0; // Counting Turning Point
whutsup 4:bf4ad2079096 30 int sysStatus=0;
whutsup 4:bf4ad2079096 31 bool mkOn = 0;
whutsup 2:b7d4195e0fea 32
whutsup 4:bf4ad2079096 33 /*----------------------- ReadData Variables ------------------------*/
whutsup 0:cbbc3528eb59 34
whutsup 4:bf4ad2079096 35 char rxData[7];
whutsup 4:bf4ad2079096 36 bool flagRx = 0;
whutsup 0:cbbc3528eb59 37
whutsup 10:3fcaf50f528f 38
whutsup 10:3fcaf50f528f 39
whutsup 10:3fcaf50f528f 40 float parA =73.671f;
whutsup 10:3fcaf50f528f 41 float parB =-1*206.25f;
whutsup 10:3fcaf50f528f 42 float parC =106.43f;
whutsup 0:cbbc3528eb59 43
whutsup 4:bf4ad2079096 44 /*----------------------- Callback Functions ------------------------*/
whutsup 0:cbbc3528eb59 45
whutsup 5:6bb52b2a79bf 46 void Test()
whutsup 5:6bb52b2a79bf 47 {
whutsup 5:6bb52b2a79bf 48 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 49 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 5:6bb52b2a79bf 50
whutsup 8:a435e7aa7a02 51 bt.printf("%1.3f\n",d2);
whutsup 5:6bb52b2a79bf 52 }
whutsup 5:6bb52b2a79bf 53
whutsup 4:bf4ad2079096 54 void ReadData()
whutsup 0:cbbc3528eb59 55 {
whutsup 4:bf4ad2079096 56 char inChar;
whutsup 8:a435e7aa7a02 57
whutsup 4:bf4ad2079096 58 static char rxCount = 0;
whutsup 4:bf4ad2079096 59 static char rxBuf[7];
whutsup 0:cbbc3528eb59 60
whutsup 4:bf4ad2079096 61 while(1 == bt.readable())
whutsup 0:cbbc3528eb59 62 {
whutsup 8:a435e7aa7a02 63
whutsup 4:bf4ad2079096 64 inChar = bt.getc();
whutsup 8:a435e7aa7a02 65
whutsup 8:a435e7aa7a02 66 bt.putc(inChar);
whutsup 8:a435e7aa7a02 67
whutsup 4:bf4ad2079096 68 if('<' == inChar)
whutsup 4:bf4ad2079096 69 {
whutsup 4:bf4ad2079096 70 rxCount = 1;
whutsup 4:bf4ad2079096 71 }
whutsup 0:cbbc3528eb59 72
whutsup 4:bf4ad2079096 73 else if (rxCount > 0 && rxCount <8)
whutsup 4:bf4ad2079096 74 {
whutsup 4:bf4ad2079096 75 rxBuf[rxCount-1] = inChar;
whutsup 4:bf4ad2079096 76 rxCount++;
whutsup 4:bf4ad2079096 77 }
whutsup 0:cbbc3528eb59 78
whutsup 4:bf4ad2079096 79 else if ( 8 == rxCount && '>' == inChar)
whutsup 4:bf4ad2079096 80 {
whutsup 4:bf4ad2079096 81 rxCount = 0;
whutsup 4:bf4ad2079096 82 flagRx = 1;
whutsup 4:bf4ad2079096 83 memcpy(rxData, rxBuf, 7);
whutsup 4:bf4ad2079096 84 }
whutsup 8:a435e7aa7a02 85 else{
whutsup 8:a435e7aa7a02 86 rxCount = 0;
whutsup 8:a435e7aa7a02 87 flagRx = 0;
whutsup 8:a435e7aa7a02 88 }
whutsup 8:a435e7aa7a02 89
whutsup 4:bf4ad2079096 90 }
whutsup 0:cbbc3528eb59 91 }
whutsup 0:cbbc3528eb59 92
whutsup 4:bf4ad2079096 93
whutsup 4:bf4ad2079096 94 void stop()
whutsup 0:cbbc3528eb59 95 {
whutsup 8:a435e7aa7a02 96 MotorTest(0);
whutsup 4:bf4ad2079096 97 }
whutsup 0:cbbc3528eb59 98
whutsup 4:bf4ad2079096 99 void ReadAng()
whutsup 4:bf4ad2079096 100 {
whutsup 4:bf4ad2079096 101 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 102 float d2 = parA*d1*d1+parB*d1+parC;
Bhoney 6:549177f76f8e 103
Bhoney 6:549177f76f8e 104 // original
Bhoney 6:549177f76f8e 105 // bt.printf("%1.2f",d2);
Bhoney 6:549177f76f8e 106
Bhoney 6:549177f76f8e 107 // fixed
whutsup 10:3fcaf50f528f 108 bt.printf("<DIO%1.2f>",d2);
Bhoney 6:549177f76f8e 109
whutsup 0:cbbc3528eb59 110 }
whutsup 0:cbbc3528eb59 111
whutsup 8:a435e7aa7a02 112 void ModeSelect(int mode)
whutsup 0:cbbc3528eb59 113 {
whutsup 4:bf4ad2079096 114
whutsup 4:bf4ad2079096 115 switch(mode)
whutsup 4:bf4ad2079096 116 {
whutsup 1:a003b900b810 117
whutsup 4:bf4ad2079096 118 case 0 : //Standard Mode
whutsup 4:bf4ad2079096 119
whutsup 4:bf4ad2079096 120 timer1.detach();
whutsup 4:bf4ad2079096 121 timer2.detach();
whutsup 4:bf4ad2079096 122 timer3.detach();
whutsup 4:bf4ad2079096 123
whutsup 4:bf4ad2079096 124 break;
whutsup 4:bf4ad2079096 125
whutsup 4:bf4ad2079096 126 case 1 : //Reset Mode
whutsup 8:a435e7aa7a02 127 {
whutsup 8:a435e7aa7a02 128 timer3.detach();
whutsup 8:a435e7aa7a02 129 timer2.detach();
whutsup 8:a435e7aa7a02 130 timer1.detach();
whutsup 8:a435e7aa7a02 131 onewayNum = 0;
whutsup 8:a435e7aa7a02 132 milLoop = 0;
whutsup 8:a435e7aa7a02 133
whutsup 8:a435e7aa7a02 134 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 135 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 136
whutsup 8:a435e7aa7a02 137 MotorTest(2);
whutsup 8:a435e7aa7a02 138 ResetTimer.attach(&stop,0.5f+d2/3.0f);
whutsup 8:a435e7aa7a02 139
whutsup 8:a435e7aa7a02 140 break;
whutsup 8:a435e7aa7a02 141 }
whutsup 4:bf4ad2079096 142
whutsup 4:bf4ad2079096 143 case 2 : //Read Force
whutsup 1:a003b900b810 144
whutsup 8:a435e7aa7a02 145 timer2.attach(&ReadForce,0.05);
whutsup 4:bf4ad2079096 146
whutsup 4:bf4ad2079096 147 break;
whutsup 4:bf4ad2079096 148
whutsup 4:bf4ad2079096 149 case 3 : //Read Angle
whutsup 1:a003b900b810 150
whutsup 4:bf4ad2079096 151 timer2.detach();
Bhoney 6:549177f76f8e 152 ReadAng(); // return only one message
whutsup 4:bf4ad2079096 153
whutsup 4:bf4ad2079096 154 break;
whutsup 4:bf4ad2079096 155
whutsup 4:bf4ad2079096 156 case 4 : //Pause Temporarily
whutsup 4:bf4ad2079096 157
whutsup 4:bf4ad2079096 158 MotorTest(0);
whutsup 4:bf4ad2079096 159
whutsup 4:bf4ad2079096 160 break;
whutsup 4:bf4ad2079096 161
whutsup 4:bf4ad2079096 162
whutsup 4:bf4ad2079096 163 }
whutsup 0:cbbc3528eb59 164 }
whutsup 0:cbbc3528eb59 165
whutsup 2:b7d4195e0fea 166
whutsup 3:b79aa7d836fb 167 void OperateLED()
whutsup 3:b79aa7d836fb 168 {
whutsup 3:b79aa7d836fb 169 led =!led;
whutsup 3:b79aa7d836fb 170 }
whutsup 0:cbbc3528eb59 171
whutsup 4:bf4ad2079096 172 void ManualMk()
whutsup 4:bf4ad2079096 173 {
whutsup 4:bf4ad2079096 174
whutsup 4:bf4ad2079096 175 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 176 {
Bhoney 6:549177f76f8e 177 bt.puts("Please Wait Moving Anybaro"); //<STA>
whutsup 4:bf4ad2079096 178 }
whutsup 4:bf4ad2079096 179 else
whutsup 4:bf4ad2079096 180 {
whutsup 4:bf4ad2079096 181 sysStatus = 6;
whutsup 4:bf4ad2079096 182 MkActionManual();
whutsup 4:bf4ad2079096 183 }
whutsup 4:bf4ad2079096 184
whutsup 4:bf4ad2079096 185
whutsup 4:bf4ad2079096 186 }
whutsup 4:bf4ad2079096 187
whutsup 4:bf4ad2079096 188 void ButtonSys()
whutsup 4:bf4ad2079096 189 {
whutsup 4:bf4ad2079096 190 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 191 {
Bhoney 6:549177f76f8e 192 bt.puts("Please Wait Moving Anybaro"); //<STA> 또는 ifdef debug 처리?
Bhoney 6:549177f76f8e 193
whutsup 4:bf4ad2079096 194 }
whutsup 4:bf4ad2079096 195 else
whutsup 4:bf4ad2079096 196 {
whutsup 4:bf4ad2079096 197 MotorButton();
whutsup 4:bf4ad2079096 198 }
whutsup 4:bf4ad2079096 199 }
Bhoney 12:75fb40f38c19 200 //---------------------- testing ------------- 190703 BH
Bhoney 12:75fb40f38c19 201 InterruptIn nFault(PB_15); // Motor Driver DRV
whutsup 13:5601fedf0e12 202 bool nFaultFlag = true;
whutsup 14:d059d99e9b5e 203
whutsup 14:d059d99e9b5e 204 //this interrupt function might not be necessary. need to be tested.
Bhoney 12:75fb40f38c19 205 void nFaultHigh(){
whutsup 13:5601fedf0e12 206 nFaultFlag = true;
Bhoney 12:75fb40f38c19 207 }
whutsup 13:5601fedf0e12 208
Bhoney 12:75fb40f38c19 209 void nFaultLow(){
whutsup 13:5601fedf0e12 210 nFaultFlag = false;
Bhoney 12:75fb40f38c19 211 }
whutsup 14:d059d99e9b5e 212
whutsup 14:d059d99e9b5e 213 DigitalOut enable(PB_12);
whutsup 14:d059d99e9b5e 214 DigitalOut nreset(PC_6);
whutsup 14:d059d99e9b5e 215 PwmOut p1(PB_13);
whutsup 14:d059d99e9b5e 216 PwmOut p2(PB_14);
whutsup 14:d059d99e9b5e 217
whutsup 14:d059d99e9b5e 218
Bhoney 12:75fb40f38c19 219 //---------------------- end ------------- 190703 BH
whutsup 0:cbbc3528eb59 220 int main()
whutsup 0:cbbc3528eb59 221 {
whutsup 4:bf4ad2079096 222 bt.baud(115200);
Bhoney 12:75fb40f38c19 223 pc.baud(115200);
Bhoney 12:75fb40f38c19 224 bt.puts("START 190703 Ver.8 \r\n");
Bhoney 12:75fb40f38c19 225 pc.puts("START 190703 Ver.8 \r\n");
Bhoney 6:549177f76f8e 226 int modeNum = 0;
Bhoney 6:549177f76f8e 227 int modeMotor = 0;
whutsup 4:bf4ad2079096 228
whutsup 8:a435e7aa7a02 229 char tmpCommand[4]={0,}; // [ADD] command
whutsup 4:bf4ad2079096 230 int rxVal;
whutsup 0:cbbc3528eb59 231
Bhoney 12:75fb40f38c19 232 //---------------------- testing ------------- 190703 BH
Bhoney 12:75fb40f38c19 233 nFault.mode(PullUp);
Bhoney 12:75fb40f38c19 234 nFault.fall(&nFaultLow);
whutsup 14:d059d99e9b5e 235
whutsup 14:d059d99e9b5e 236 //this interrupt function might not be necessary. need to be tested.
whutsup 14:d059d99e9b5e 237 nFault.rise(&nFaultHigh);
whutsup 14:d059d99e9b5e 238
Bhoney 12:75fb40f38c19 239 //---------------------- end ------------- 190703 BH
Bhoney 12:75fb40f38c19 240
whutsup 0:cbbc3528eb59 241 up.mode(PullNone);
whutsup 0:cbbc3528eb59 242 down.mode(PullNone);
whutsup 8:a435e7aa7a02 243 bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
whutsup 2:b7d4195e0fea 244
whutsup 4:bf4ad2079096 245 up.fall(&ButtonSys);
whutsup 4:bf4ad2079096 246 up.rise(&ButtonSys);
whutsup 4:bf4ad2079096 247 down.fall(&ButtonSys);
whutsup 4:bf4ad2079096 248 down.rise(&ButtonSys);
whutsup 0:cbbc3528eb59 249
whutsup 10:3fcaf50f528f 250 bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
whutsup 3:b79aa7d836fb 251
whutsup 9:7d6fa62f9022 252 timer0.attach(&OperateLED,0.2);
whutsup 10:3fcaf50f528f 253 // timer4.attach(&Test,0.1);
whutsup 4:bf4ad2079096 254 bt.attach(&ReadData, Serial::RxIrq);
whutsup 3:b79aa7d836fb 255
whutsup 3:b79aa7d836fb 256
whutsup 0:cbbc3528eb59 257 while(1)
whutsup 0:cbbc3528eb59 258 {
whutsup 4:bf4ad2079096 259
whutsup 14:d059d99e9b5e 260 //---------------------- need to be tested ------------- BH
whutsup 14:d059d99e9b5e 261 if(!nFaultFlag)
whutsup 14:d059d99e9b5e 262 {
whutsup 14:d059d99e9b5e 263
whutsup 14:d059d99e9b5e 264 // test below code (without rise interrupt callback)
whutsup 14:d059d99e9b5e 265 // nFaultFlag = true;
whutsup 14:d059d99e9b5e 266 enable = 0;
whutsup 14:d059d99e9b5e 267 nreset = 0;
whutsup 14:d059d99e9b5e 268 p1=0;
whutsup 14:d059d99e9b5e 269 p2=0;
whutsup 14:d059d99e9b5e 270 wait(0.01f);
whutsup 14:d059d99e9b5e 271 }
whutsup 13:5601fedf0e12 272 //---------------------- end ------------- 190703 BH
whutsup 13:5601fedf0e12 273
whutsup 4:bf4ad2079096 274 if (1 == flagRx)
whutsup 4:bf4ad2079096 275 {
whutsup 8:a435e7aa7a02 276
whutsup 4:bf4ad2079096 277 flagRx = 0;
whutsup 4:bf4ad2079096 278 tmpCommand[0] = rxData[0];
whutsup 4:bf4ad2079096 279 tmpCommand[1] = rxData[1];
whutsup 4:bf4ad2079096 280 tmpCommand[2] = rxData[2];
whutsup 4:bf4ad2079096 281 rxVal = atoi(rxData+3);
whutsup 0:cbbc3528eb59 282
whutsup 4:bf4ad2079096 283
whutsup 10:3fcaf50f528f 284 if (0 == strcmp(tmpCommand,"MOD"))
whutsup 4:bf4ad2079096 285 {
Bhoney 6:549177f76f8e 286 modeNum = rxVal; // 1 ~ 6
Bhoney 6:549177f76f8e 287 sysStatus = rxVal; // Why this value are assigned directly?
Bhoney 6:549177f76f8e 288 // assigning sys variable after checking condition is correct progress, i think.
whutsup 4:bf4ad2079096 289 ModeSelect(modeNum);
whutsup 4:bf4ad2079096 290 }
whutsup 0:cbbc3528eb59 291
whutsup 4:bf4ad2079096 292 else if (0 == strcmp(tmpCommand,"MOT"))
whutsup 10:3fcaf50f528f 293 {
whutsup 4:bf4ad2079096 294 modeMotor = rxVal;
Bhoney 6:549177f76f8e 295 MotorTest(modeMotor);
whutsup 1:a003b900b810 296 }
whutsup 1:a003b900b810 297
whutsup 4:bf4ad2079096 298 else if (0 == strcmp(tmpCommand,"POS"))
whutsup 4:bf4ad2079096 299 {
whutsup 8:a435e7aa7a02 300 // errorPrevious = 0;
whutsup 4:bf4ad2079096 301 targetDis = rxVal;
whutsup 8:a435e7aa7a02 302 // float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 303 // float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 304 // errorPrevious = targetDis - d2;
whutsup 8:a435e7aa7a02 305
whutsup 8:a435e7aa7a02 306 timer1.attach(&ControlAng,0.01);
whutsup 4:bf4ad2079096 307 }
whutsup 4:bf4ad2079096 308
whutsup 4:bf4ad2079096 309 if (0 == strcmp(tmpCommand,"MIL"))
whutsup 1:a003b900b810 310 {
whutsup 4:bf4ad2079096 311 sysStatus = 5;
whutsup 4:bf4ad2079096 312 targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
whutsup 4:bf4ad2079096 313 milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
whutsup 4:bf4ad2079096 314 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 315
whutsup 4:bf4ad2079096 316 if ( 0 == mkOn) mkOn = 1;
whutsup 4:bf4ad2079096 317
whutsup 1:a003b900b810 318 }
whutsup 4:bf4ad2079096 319
whutsup 4:bf4ad2079096 320
whutsup 4:bf4ad2079096 321 }
whutsup 2:b7d4195e0fea 322
whutsup 0:cbbc3528eb59 323
whutsup 0:cbbc3528eb59 324 }
whutsup 0:cbbc3528eb59 325
whutsup 0:cbbc3528eb59 326 }