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@13:5601fedf0e12, 2019-07-03 (annotated)

Committer:
whutsup
Date:
Wed Jul 03 08:32:41 2019 +0000
Revision:
13:5601fedf0e12
Parent:
12:75fb40f38c19
Child:
14:d059d99e9b5e
demo ver.1

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;
Bhoney 12:75fb40f38c19 203 void nFaultHigh(){
whutsup 13:5601fedf0e12 204 nFaultFlag = true;
whutsup 13:5601fedf0e12 205 // pc.puts("nFault HIGH\r\n");
whutsup 13:5601fedf0e12 206 // bt.puts("nFault HIGH\r\n");
Bhoney 12:75fb40f38c19 207 }
whutsup 13:5601fedf0e12 208
Bhoney 12:75fb40f38c19 209 void nFaultLow(){
whutsup 13:5601fedf0e12 210 nFaultFlag = false;
whutsup 13:5601fedf0e12 211 // pc.puts("nFault LOW\r\n");
whutsup 13:5601fedf0e12 212 // bt.puts("nFault LOW\r\n");
Bhoney 12:75fb40f38c19 213 }
Bhoney 12:75fb40f38c19 214 //---------------------- end ------------- 190703 BH
whutsup 0:cbbc3528eb59 215 int main()
whutsup 0:cbbc3528eb59 216 {
whutsup 4:bf4ad2079096 217 bt.baud(115200);
Bhoney 12:75fb40f38c19 218 pc.baud(115200);
Bhoney 12:75fb40f38c19 219 bt.puts("START 190703 Ver.8 \r\n");
Bhoney 12:75fb40f38c19 220 pc.puts("START 190703 Ver.8 \r\n");
Bhoney 6:549177f76f8e 221 int modeNum = 0;
Bhoney 6:549177f76f8e 222 int modeMotor = 0;
whutsup 4:bf4ad2079096 223
whutsup 8:a435e7aa7a02 224 char tmpCommand[4]={0,}; // [ADD] command
whutsup 4:bf4ad2079096 225 int rxVal;
whutsup 0:cbbc3528eb59 226
Bhoney 12:75fb40f38c19 227 //---------------------- testing ------------- 190703 BH
Bhoney 12:75fb40f38c19 228 nFault.mode(PullUp);
Bhoney 12:75fb40f38c19 229 nFault.fall(&nFaultLow);
Bhoney 12:75fb40f38c19 230 nFault.rise(&nFaultHigh);
Bhoney 12:75fb40f38c19 231
Bhoney 12:75fb40f38c19 232 //---------------------- end ------------- 190703 BH
Bhoney 12:75fb40f38c19 233
whutsup 0:cbbc3528eb59 234 up.mode(PullNone);
whutsup 0:cbbc3528eb59 235 down.mode(PullNone);
whutsup 8:a435e7aa7a02 236 bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
whutsup 2:b7d4195e0fea 237
whutsup 4:bf4ad2079096 238 up.fall(&ButtonSys);
whutsup 4:bf4ad2079096 239 up.rise(&ButtonSys);
whutsup 4:bf4ad2079096 240 down.fall(&ButtonSys);
whutsup 4:bf4ad2079096 241 down.rise(&ButtonSys);
whutsup 0:cbbc3528eb59 242
whutsup 10:3fcaf50f528f 243 bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
whutsup 3:b79aa7d836fb 244
whutsup 9:7d6fa62f9022 245 timer0.attach(&OperateLED,0.2);
whutsup 10:3fcaf50f528f 246 // timer4.attach(&Test,0.1);
whutsup 4:bf4ad2079096 247 bt.attach(&ReadData, Serial::RxIrq);
whutsup 3:b79aa7d836fb 248
whutsup 3:b79aa7d836fb 249
whutsup 0:cbbc3528eb59 250 while(1)
whutsup 0:cbbc3528eb59 251 {
whutsup 4:bf4ad2079096 252
whutsup 13:5601fedf0e12 253 //---------------------- testing ------------- 190703 BH
whutsup 13:5601fedf0e12 254
whutsup 13:5601fedf0e12 255 // if (1 == flagRx)
whutsup 13:5601fedf0e12 256
whutsup 13:5601fedf0e12 257 //---------------------- end ------------- 190703 BH
whutsup 13:5601fedf0e12 258
whutsup 4:bf4ad2079096 259 if (1 == flagRx)
whutsup 4:bf4ad2079096 260 {
whutsup 8:a435e7aa7a02 261
whutsup 4:bf4ad2079096 262 flagRx = 0;
whutsup 4:bf4ad2079096 263 tmpCommand[0] = rxData[0];
whutsup 4:bf4ad2079096 264 tmpCommand[1] = rxData[1];
whutsup 4:bf4ad2079096 265 tmpCommand[2] = rxData[2];
whutsup 4:bf4ad2079096 266 rxVal = atoi(rxData+3);
whutsup 0:cbbc3528eb59 267
whutsup 4:bf4ad2079096 268
whutsup 10:3fcaf50f528f 269 if (0 == strcmp(tmpCommand,"MOD"))
whutsup 4:bf4ad2079096 270 {
Bhoney 6:549177f76f8e 271 modeNum = rxVal; // 1 ~ 6
Bhoney 6:549177f76f8e 272 sysStatus = rxVal; // Why this value are assigned directly?
Bhoney 6:549177f76f8e 273 // assigning sys variable after checking condition is correct progress, i think.
whutsup 4:bf4ad2079096 274 ModeSelect(modeNum);
whutsup 4:bf4ad2079096 275 }
whutsup 0:cbbc3528eb59 276
whutsup 4:bf4ad2079096 277 else if (0 == strcmp(tmpCommand,"MOT"))
whutsup 10:3fcaf50f528f 278 {
whutsup 4:bf4ad2079096 279 modeMotor = rxVal;
Bhoney 6:549177f76f8e 280 MotorTest(modeMotor);
whutsup 1:a003b900b810 281 }
whutsup 1:a003b900b810 282
whutsup 4:bf4ad2079096 283 else if (0 == strcmp(tmpCommand,"POS"))
whutsup 4:bf4ad2079096 284 {
whutsup 8:a435e7aa7a02 285 // errorPrevious = 0;
whutsup 4:bf4ad2079096 286 targetDis = rxVal;
whutsup 8:a435e7aa7a02 287 // float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 288 // float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 289 // errorPrevious = targetDis - d2;
whutsup 8:a435e7aa7a02 290
whutsup 8:a435e7aa7a02 291 timer1.attach(&ControlAng,0.01);
whutsup 4:bf4ad2079096 292 }
whutsup 4:bf4ad2079096 293
whutsup 4:bf4ad2079096 294 if (0 == strcmp(tmpCommand,"MIL"))
whutsup 1:a003b900b810 295 {
whutsup 4:bf4ad2079096 296 sysStatus = 5;
whutsup 4:bf4ad2079096 297 targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
whutsup 4:bf4ad2079096 298 milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
whutsup 4:bf4ad2079096 299 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 300
whutsup 4:bf4ad2079096 301 if ( 0 == mkOn) mkOn = 1;
whutsup 4:bf4ad2079096 302
whutsup 1:a003b900b810 303 }
whutsup 4:bf4ad2079096 304
whutsup 4:bf4ad2079096 305
whutsup 4:bf4ad2079096 306 }
whutsup 2:b7d4195e0fea 307
whutsup 0:cbbc3528eb59 308
whutsup 0:cbbc3528eb59 309 }
whutsup 0:cbbc3528eb59 310
whutsup 0:cbbc3528eb59 311 }