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@19:67584cb64b9c, 2019-08-22 (annotated)

Committer:
Bhoney
Date:
Thu Aug 22 08:36:15 2019 +0000
Revision:
19:67584cb64b9c
Parent:
15:e5e34512a00e 
QUALIFYING_MODE is added (Qtimer, Qtime, Qflag, Qfunc for btn)

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