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@9:7d6fa62f9022, 2019-06-25 (annotated)

Committer:
whutsup
Date:
Tue Jun 25 02:10:31 2019 +0000
Revision:
9:7d6fa62f9022
Parent:
8:a435e7aa7a02
Child:
10:3fcaf50f528f
190625 updated ver

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 #define DEBUG
whutsup 0:cbbc3528eb59 6
whutsup 4:bf4ad2079096 7 Serial bt(PA_9,PA_10); // USART1 Bluetooth (Tx, Rx)
whutsup 4:bf4ad2079096 8 Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
whutsup 2:b7d4195e0fea 9
whutsup 4:bf4ad2079096 10 InterruptIn up(PC_8); // Button Interrupt Motor control
whutsup 4:bf4ad2079096 11 InterruptIn down(PC_9);
whutsup 8:a435e7aa7a02 12 InterruptIn bt_ml(PC_10); // Millking action button
whutsup 4:bf4ad2079096 13
whutsup 9:7d6fa62f9022 14 DigitalOut led(PA_5); // Operating LED signal
whutsup 4:bf4ad2079096 15
whutsup 4:bf4ad2079096 16 AnalogIn disSensor(PA_7); // Distance Sensor
whutsup 0:cbbc3528eb59 17
whutsup 4:bf4ad2079096 18 Timeout ResetTimer; // Reset angle 0 position
whutsup 4:bf4ad2079096 19
whutsup 4:bf4ad2079096 20 Ticker timer0; // Operating MCU LED on
whutsup 4:bf4ad2079096 21 Ticker timer1; // Control Angle By Using Distance Sensor
whutsup 4:bf4ad2079096 22 Ticker timer2; // Force Sensor
whutsup 4:bf4ad2079096 23 Ticker timer3; // Millking Action Mode
whutsup 5:6bb52b2a79bf 24 Ticker timer4;
whutsup 0:cbbc3528eb59 25
whutsup 4:bf4ad2079096 26 int targetDis; // Target Of Distance
whutsup 4:bf4ad2079096 27 int milLoop; // Number of Millking Action
whutsup 8:a435e7aa7a02 28 //float errorPrevious;
whutsup 8:a435e7aa7a02 29
whutsup 4:bf4ad2079096 30 int onewayNum=0; // Counting Turning Point
whutsup 4:bf4ad2079096 31 int sysStatus=0;
whutsup 4:bf4ad2079096 32 bool mkOn = 0;
whutsup 2:b7d4195e0fea 33
whutsup 4:bf4ad2079096 34 /*----------------------- ReadData Variables ------------------------*/
whutsup 0:cbbc3528eb59 35
whutsup 4:bf4ad2079096 36 char rxData[7];
whutsup 4:bf4ad2079096 37 bool flagRx = 0;
whutsup 0:cbbc3528eb59 38
whutsup 5:6bb52b2a79bf 39 #define parA 343.75f // parmeter of equation (values of distance sensor)
whutsup 5:6bb52b2a79bf 40 #define parB -1*508.08f
whutsup 5:6bb52b2a79bf 41 #define parC 166.67f
whutsup 0:cbbc3528eb59 42
whutsup 4:bf4ad2079096 43 /*----------------------- Callback Functions ------------------------*/
whutsup 0:cbbc3528eb59 44
whutsup 5:6bb52b2a79bf 45 void Test()
whutsup 5:6bb52b2a79bf 46 {
whutsup 5:6bb52b2a79bf 47 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 48 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 5:6bb52b2a79bf 49
whutsup 8:a435e7aa7a02 50 bt.printf("%1.3f\n",d2);
whutsup 5:6bb52b2a79bf 51 }
whutsup 5:6bb52b2a79bf 52
whutsup 4:bf4ad2079096 53 void ReadData()
whutsup 0:cbbc3528eb59 54 {
whutsup 4:bf4ad2079096 55 char inChar;
whutsup 8:a435e7aa7a02 56
whutsup 4:bf4ad2079096 57 static char rxCount = 0;
whutsup 4:bf4ad2079096 58 static char rxBuf[7];
whutsup 0:cbbc3528eb59 59
whutsup 4:bf4ad2079096 60 while(1 == bt.readable())
whutsup 0:cbbc3528eb59 61 {
whutsup 8:a435e7aa7a02 62
whutsup 4:bf4ad2079096 63 inChar = bt.getc();
whutsup 8:a435e7aa7a02 64
whutsup 8:a435e7aa7a02 65 bt.putc(inChar);
whutsup 8:a435e7aa7a02 66
whutsup 4:bf4ad2079096 67 if('<' == inChar)
whutsup 4:bf4ad2079096 68 {
whutsup 4:bf4ad2079096 69 rxCount = 1;
whutsup 4:bf4ad2079096 70 }
whutsup 0:cbbc3528eb59 71
whutsup 4:bf4ad2079096 72 else if (rxCount > 0 && rxCount <8)
whutsup 4:bf4ad2079096 73 {
whutsup 4:bf4ad2079096 74 rxBuf[rxCount-1] = inChar;
whutsup 4:bf4ad2079096 75 rxCount++;
whutsup 4:bf4ad2079096 76 }
whutsup 0:cbbc3528eb59 77
whutsup 4:bf4ad2079096 78 else if ( 8 == rxCount && '>' == inChar)
whutsup 4:bf4ad2079096 79 {
whutsup 4:bf4ad2079096 80 rxCount = 0;
whutsup 4:bf4ad2079096 81 flagRx = 1;
whutsup 4:bf4ad2079096 82 memcpy(rxData, rxBuf, 7);
whutsup 4:bf4ad2079096 83 }
whutsup 8:a435e7aa7a02 84 else{
whutsup 8:a435e7aa7a02 85 rxCount = 0;
whutsup 8:a435e7aa7a02 86 flagRx = 0;
whutsup 8:a435e7aa7a02 87 }
whutsup 8:a435e7aa7a02 88
whutsup 4:bf4ad2079096 89 }
whutsup 0:cbbc3528eb59 90 }
whutsup 0:cbbc3528eb59 91
whutsup 4:bf4ad2079096 92
whutsup 0:cbbc3528eb59 93 void ControlLED(int mode)
whutsup 0:cbbc3528eb59 94 {
whutsup 4:bf4ad2079096 95
whutsup 0:cbbc3528eb59 96
whutsup 0:cbbc3528eb59 97 switch (mode)
whutsup 0:cbbc3528eb59 98 {
whutsup 0:cbbc3528eb59 99 case 0 :
whutsup 4:bf4ad2079096 100 timer0.detach();
whutsup 0:cbbc3528eb59 101 led = 0;
whutsup 0:cbbc3528eb59 102
whutsup 0:cbbc3528eb59 103 break;
whutsup 0:cbbc3528eb59 104
whutsup 0:cbbc3528eb59 105 case 1 :
whutsup 4:bf4ad2079096 106 timer0.detach();
whutsup 0:cbbc3528eb59 107 led = 1;
whutsup 0:cbbc3528eb59 108
whutsup 0:cbbc3528eb59 109 break;
whutsup 0:cbbc3528eb59 110
whutsup 0:cbbc3528eb59 111 case 2 :
whutsup 4:bf4ad2079096 112 bt.puts("ANYBARO");
whutsup 0:cbbc3528eb59 113
Bhoney 6:549177f76f8e 114 break;
Bhoney 6:549177f76f8e 115
Bhoney 6:549177f76f8e 116 default:
Bhoney 6:549177f76f8e 117 break;
whutsup 0:cbbc3528eb59 118 }
whutsup 0:cbbc3528eb59 119
whutsup 0:cbbc3528eb59 120 }
whutsup 0:cbbc3528eb59 121
whutsup 4:bf4ad2079096 122 void stop()
whutsup 0:cbbc3528eb59 123 {
whutsup 8:a435e7aa7a02 124 MotorTest(0);
whutsup 4:bf4ad2079096 125 }
whutsup 0:cbbc3528eb59 126
whutsup 4:bf4ad2079096 127 void ReadAng()
whutsup 4:bf4ad2079096 128 {
whutsup 4:bf4ad2079096 129 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 130 float d2 = parA*d1*d1+parB*d1+parC;
Bhoney 6:549177f76f8e 131
Bhoney 6:549177f76f8e 132 // original
Bhoney 6:549177f76f8e 133 // bt.printf("%1.2f",d2);
Bhoney 6:549177f76f8e 134
Bhoney 6:549177f76f8e 135 // fixed
Bhoney 6:549177f76f8e 136 bt.printf("<DIO%1.4f>",d2);
Bhoney 6:549177f76f8e 137
whutsup 0:cbbc3528eb59 138 }
whutsup 0:cbbc3528eb59 139
whutsup 8:a435e7aa7a02 140 void ModeSelect(int mode)
whutsup 0:cbbc3528eb59 141 {
whutsup 4:bf4ad2079096 142
whutsup 4:bf4ad2079096 143 switch(mode)
whutsup 4:bf4ad2079096 144 {
whutsup 1:a003b900b810 145
whutsup 4:bf4ad2079096 146 case 0 : //Standard Mode
whutsup 4:bf4ad2079096 147
whutsup 4:bf4ad2079096 148 timer1.detach();
whutsup 4:bf4ad2079096 149 timer2.detach();
whutsup 4:bf4ad2079096 150 timer3.detach();
whutsup 4:bf4ad2079096 151
whutsup 4:bf4ad2079096 152 break;
whutsup 4:bf4ad2079096 153
whutsup 4:bf4ad2079096 154 case 1 : //Reset Mode
whutsup 8:a435e7aa7a02 155 {
whutsup 8:a435e7aa7a02 156 timer3.detach();
whutsup 8:a435e7aa7a02 157 timer2.detach();
whutsup 8:a435e7aa7a02 158 timer1.detach();
whutsup 8:a435e7aa7a02 159 onewayNum = 0;
whutsup 8:a435e7aa7a02 160 milLoop = 0;
whutsup 8:a435e7aa7a02 161
whutsup 8:a435e7aa7a02 162 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 163 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 164
whutsup 8:a435e7aa7a02 165 MotorTest(2);
whutsup 8:a435e7aa7a02 166 ResetTimer.attach(&stop,0.5f+d2/3.0f);
whutsup 8:a435e7aa7a02 167
whutsup 8:a435e7aa7a02 168 break;
whutsup 8:a435e7aa7a02 169 }
whutsup 4:bf4ad2079096 170
whutsup 4:bf4ad2079096 171 case 2 : //Read Force
whutsup 1:a003b900b810 172
whutsup 8:a435e7aa7a02 173 timer2.attach(&ReadForce,0.05);
whutsup 4:bf4ad2079096 174
whutsup 4:bf4ad2079096 175 break;
whutsup 4:bf4ad2079096 176
whutsup 4:bf4ad2079096 177 case 3 : //Read Angle
whutsup 1:a003b900b810 178
whutsup 4:bf4ad2079096 179 timer2.detach();
Bhoney 6:549177f76f8e 180 ReadAng(); // return only one message
whutsup 4:bf4ad2079096 181
whutsup 4:bf4ad2079096 182 break;
whutsup 4:bf4ad2079096 183
whutsup 4:bf4ad2079096 184 case 4 : //Pause Temporarily
whutsup 4:bf4ad2079096 185
whutsup 4:bf4ad2079096 186 MotorTest(0);
whutsup 4:bf4ad2079096 187
whutsup 4:bf4ad2079096 188 break;
whutsup 4:bf4ad2079096 189
whutsup 4:bf4ad2079096 190
whutsup 4:bf4ad2079096 191 }
whutsup 0:cbbc3528eb59 192 }
whutsup 0:cbbc3528eb59 193
whutsup 2:b7d4195e0fea 194
whutsup 3:b79aa7d836fb 195 void OperateLED()
whutsup 3:b79aa7d836fb 196 {
whutsup 3:b79aa7d836fb 197 led =!led;
whutsup 3:b79aa7d836fb 198 }
whutsup 0:cbbc3528eb59 199
whutsup 4:bf4ad2079096 200 void ManualMk()
whutsup 4:bf4ad2079096 201 {
whutsup 4:bf4ad2079096 202
whutsup 4:bf4ad2079096 203 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 204 {
Bhoney 6:549177f76f8e 205 bt.puts("Please Wait Moving Anybaro"); //<STA>
whutsup 4:bf4ad2079096 206 }
whutsup 4:bf4ad2079096 207 else
whutsup 4:bf4ad2079096 208 {
whutsup 4:bf4ad2079096 209 sysStatus = 6;
whutsup 4:bf4ad2079096 210 MkActionManual();
whutsup 4:bf4ad2079096 211 }
whutsup 4:bf4ad2079096 212
whutsup 4:bf4ad2079096 213
whutsup 4:bf4ad2079096 214 }
whutsup 4:bf4ad2079096 215
whutsup 4:bf4ad2079096 216 void ButtonSys()
whutsup 4:bf4ad2079096 217 {
whutsup 4:bf4ad2079096 218 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 219 {
Bhoney 6:549177f76f8e 220 bt.puts("Please Wait Moving Anybaro"); //<STA> 또는 ifdef debug 처리?
Bhoney 6:549177f76f8e 221
whutsup 4:bf4ad2079096 222 }
whutsup 4:bf4ad2079096 223 else
whutsup 4:bf4ad2079096 224 {
whutsup 4:bf4ad2079096 225 MotorButton();
whutsup 4:bf4ad2079096 226 }
whutsup 4:bf4ad2079096 227 }
whutsup 4:bf4ad2079096 228
whutsup 0:cbbc3528eb59 229 int main()
whutsup 0:cbbc3528eb59 230 {
whutsup 4:bf4ad2079096 231 bt.baud(115200);
whutsup 8:a435e7aa7a02 232 // pc.baud(115200);
whutsup 8:a435e7aa7a02 233 bt.puts("START 181228 Ver.6 \n");
Bhoney 6:549177f76f8e 234 int modeNum = 0;
whutsup 4:bf4ad2079096 235 int modeLED = 0;
Bhoney 6:549177f76f8e 236 int modeMotor = 0;
whutsup 4:bf4ad2079096 237
whutsup 8:a435e7aa7a02 238 char tmpCommand[4]={0,}; // [ADD] command
whutsup 4:bf4ad2079096 239 int rxVal;
whutsup 0:cbbc3528eb59 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 8:a435e7aa7a02 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 5:6bb52b2a79bf 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 4:bf4ad2079096 260 if (1 == flagRx)
whutsup 4:bf4ad2079096 261 {
whutsup 8:a435e7aa7a02 262
whutsup 4:bf4ad2079096 263 flagRx = 0;
whutsup 4:bf4ad2079096 264 tmpCommand[0] = rxData[0];
whutsup 4:bf4ad2079096 265 tmpCommand[1] = rxData[1];
whutsup 4:bf4ad2079096 266 tmpCommand[2] = rxData[2];
whutsup 4:bf4ad2079096 267 rxVal = atoi(rxData+3);
whutsup 0:cbbc3528eb59 268
whutsup 4:bf4ad2079096 269 if (0 == strcmp(tmpCommand,"LED"))
whutsup 0:cbbc3528eb59 270 {
whutsup 4:bf4ad2079096 271 #ifdef DEBUG
whutsup 4:bf4ad2079096 272 bt.puts("\nLED CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 273 #endif
Bhoney 6:549177f76f8e 274 modeLED = rxVal; // if(modeLED != 0 || modeLED != 1 || modeLED != 2)
whutsup 0:cbbc3528eb59 275 ControlLED(modeLED);
whutsup 0:cbbc3528eb59 276 }
whutsup 4:bf4ad2079096 277
whutsup 4:bf4ad2079096 278 else if (0 == strcmp(tmpCommand,"MOD"))
whutsup 4:bf4ad2079096 279 {
whutsup 4:bf4ad2079096 280 #ifdef DEBUG
whutsup 4:bf4ad2079096 281 bt.puts("\nMODE SELECT!!\n");
whutsup 4:bf4ad2079096 282 #endif
Bhoney 6:549177f76f8e 283 modeNum = rxVal; // 1 ~ 6
Bhoney 6:549177f76f8e 284 sysStatus = rxVal; // Why this value are assigned directly?
Bhoney 6:549177f76f8e 285 // assigning sys variable after checking condition is correct progress, i think.
whutsup 4:bf4ad2079096 286 ModeSelect(modeNum);
whutsup 4:bf4ad2079096 287 }
whutsup 0:cbbc3528eb59 288
whutsup 4:bf4ad2079096 289 else if (0 == strcmp(tmpCommand,"MOT"))
whutsup 4:bf4ad2079096 290 {
whutsup 4:bf4ad2079096 291 #ifdef DEBUG
whutsup 4:bf4ad2079096 292 bt.puts("\nMOTOR TEST CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 293 #endif
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 #ifdef DEBUG
whutsup 4:bf4ad2079096 302 bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
Bhoney 6:549177f76f8e 303 #endif
whutsup 4:bf4ad2079096 304 targetDis = rxVal;
whutsup 8:a435e7aa7a02 305 // float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 306 // float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 307 // errorPrevious = targetDis - d2;
whutsup 8:a435e7aa7a02 308
whutsup 8:a435e7aa7a02 309 timer1.attach(&ControlAng,0.01);
whutsup 4:bf4ad2079096 310 }
whutsup 4:bf4ad2079096 311
whutsup 4:bf4ad2079096 312 if (0 == strcmp(tmpCommand,"MIL"))
whutsup 1:a003b900b810 313 {
whutsup 4:bf4ad2079096 314 sysStatus = 5;
whutsup 4:bf4ad2079096 315 #ifdef DEBUG
whutsup 4:bf4ad2079096 316 bt.puts("\nMILLKING ACTION MODE!!\n");
whutsup 4:bf4ad2079096 317 #endif
whutsup 4:bf4ad2079096 318 targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
whutsup 4:bf4ad2079096 319 milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
whutsup 4:bf4ad2079096 320 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 321
whutsup 4:bf4ad2079096 322 if ( 0 == mkOn) mkOn = 1;
whutsup 4:bf4ad2079096 323
whutsup 1:a003b900b810 324 }
whutsup 4:bf4ad2079096 325
whutsup 4:bf4ad2079096 326
whutsup 4:bf4ad2079096 327 }
whutsup 2:b7d4195e0fea 328
whutsup 0:cbbc3528eb59 329
whutsup 0:cbbc3528eb59 330 }
whutsup 0:cbbc3528eb59 331
whutsup 0:cbbc3528eb59 332 }