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@10:3fcaf50f528f, 2019-07-02 (annotated)

Committer:
whutsup
Date:
Tue Jul 02 04:44:35 2019 +0000
Revision:
10:3fcaf50f528f
Parent:
9:7d6fa62f9022
Child:
11:5f05b14649ee
share latest version of this repo from sungho's account

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)
whutsup 4:bf4ad2079096 6 Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
whutsup 2:b7d4195e0fea 7
whutsup 4:bf4ad2079096 8 InterruptIn up(PC_8); // Button Interrupt Motor control
whutsup 4:bf4ad2079096 9 InterruptIn down(PC_9);
whutsup 8:a435e7aa7a02 10 InterruptIn bt_ml(PC_10); // Millking action button
whutsup 4:bf4ad2079096 11
whutsup 9:7d6fa62f9022 12 DigitalOut led(PA_5); // Operating LED signal
whutsup 4:bf4ad2079096 13
whutsup 4:bf4ad2079096 14 AnalogIn disSensor(PA_7); // Distance Sensor
whutsup 0:cbbc3528eb59 15
whutsup 4:bf4ad2079096 16 Timeout ResetTimer; // Reset angle 0 position
whutsup 4:bf4ad2079096 17
whutsup 10:3fcaf50f528f 18 Ticker timer0;
whutsup 4:bf4ad2079096 19 Ticker timer1; // Control Angle By Using Distance Sensor
whutsup 4:bf4ad2079096 20 Ticker timer2; // Force Sensor
whutsup 4:bf4ad2079096 21 Ticker timer3; // Millking Action Mode
whutsup 5:6bb52b2a79bf 22 Ticker timer4;
whutsup 0:cbbc3528eb59 23
whutsup 4:bf4ad2079096 24 int targetDis; // Target Of Distance
whutsup 4:bf4ad2079096 25 int milLoop; // Number of Millking Action
whutsup 8:a435e7aa7a02 26 //float errorPrevious;
whutsup 8:a435e7aa7a02 27
whutsup 4:bf4ad2079096 28 int onewayNum=0; // Counting Turning Point
whutsup 4:bf4ad2079096 29 int sysStatus=0;
whutsup 4:bf4ad2079096 30 bool mkOn = 0;
whutsup 2:b7d4195e0fea 31
whutsup 4:bf4ad2079096 32 /*----------------------- ReadData Variables ------------------------*/
whutsup 0:cbbc3528eb59 33
whutsup 4:bf4ad2079096 34 char rxData[7];
whutsup 4:bf4ad2079096 35 bool flagRx = 0;
whutsup 0:cbbc3528eb59 36
whutsup 10:3fcaf50f528f 37
whutsup 10:3fcaf50f528f 38
whutsup 10:3fcaf50f528f 39 float parA =73.671f;
whutsup 10:3fcaf50f528f 40 float parB =-1*206.25f;
whutsup 10:3fcaf50f528f 41 float parC =106.43f;
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 4:bf4ad2079096 93 void stop()
whutsup 0:cbbc3528eb59 94 {
whutsup 8:a435e7aa7a02 95 MotorTest(0);
whutsup 4:bf4ad2079096 96 }
whutsup 0:cbbc3528eb59 97
whutsup 4:bf4ad2079096 98 void ReadAng()
whutsup 4:bf4ad2079096 99 {
whutsup 4:bf4ad2079096 100 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 101 float d2 = parA*d1*d1+parB*d1+parC;
Bhoney 6:549177f76f8e 102
Bhoney 6:549177f76f8e 103 // original
Bhoney 6:549177f76f8e 104 // bt.printf("%1.2f",d2);
Bhoney 6:549177f76f8e 105
Bhoney 6:549177f76f8e 106 // fixed
whutsup 10:3fcaf50f528f 107 bt.printf("<DIO%1.2f>",d2);
Bhoney 6:549177f76f8e 108
whutsup 0:cbbc3528eb59 109 }
whutsup 0:cbbc3528eb59 110
whutsup 8:a435e7aa7a02 111 void ModeSelect(int mode)
whutsup 0:cbbc3528eb59 112 {
whutsup 4:bf4ad2079096 113
whutsup 4:bf4ad2079096 114 switch(mode)
whutsup 4:bf4ad2079096 115 {
whutsup 1:a003b900b810 116
whutsup 4:bf4ad2079096 117 case 0 : //Standard Mode
whutsup 4:bf4ad2079096 118
whutsup 4:bf4ad2079096 119 timer1.detach();
whutsup 4:bf4ad2079096 120 timer2.detach();
whutsup 4:bf4ad2079096 121 timer3.detach();
whutsup 4:bf4ad2079096 122
whutsup 4:bf4ad2079096 123 break;
whutsup 4:bf4ad2079096 124
whutsup 4:bf4ad2079096 125 case 1 : //Reset Mode
whutsup 8:a435e7aa7a02 126 {
whutsup 8:a435e7aa7a02 127 timer3.detach();
whutsup 8:a435e7aa7a02 128 timer2.detach();
whutsup 8:a435e7aa7a02 129 timer1.detach();
whutsup 8:a435e7aa7a02 130 onewayNum = 0;
whutsup 8:a435e7aa7a02 131 milLoop = 0;
whutsup 8:a435e7aa7a02 132
whutsup 8:a435e7aa7a02 133 float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 134 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 135
whutsup 8:a435e7aa7a02 136 MotorTest(2);
whutsup 8:a435e7aa7a02 137 ResetTimer.attach(&stop,0.5f+d2/3.0f);
whutsup 8:a435e7aa7a02 138
whutsup 8:a435e7aa7a02 139 break;
whutsup 8:a435e7aa7a02 140 }
whutsup 4:bf4ad2079096 141
whutsup 4:bf4ad2079096 142 case 2 : //Read Force
whutsup 1:a003b900b810 143
whutsup 8:a435e7aa7a02 144 timer2.attach(&ReadForce,0.05);
whutsup 4:bf4ad2079096 145
whutsup 4:bf4ad2079096 146 break;
whutsup 4:bf4ad2079096 147
whutsup 4:bf4ad2079096 148 case 3 : //Read Angle
whutsup 1:a003b900b810 149
whutsup 4:bf4ad2079096 150 timer2.detach();
Bhoney 6:549177f76f8e 151 ReadAng(); // return only one message
whutsup 4:bf4ad2079096 152
whutsup 4:bf4ad2079096 153 break;
whutsup 4:bf4ad2079096 154
whutsup 4:bf4ad2079096 155 case 4 : //Pause Temporarily
whutsup 4:bf4ad2079096 156
whutsup 4:bf4ad2079096 157 MotorTest(0);
whutsup 4:bf4ad2079096 158
whutsup 4:bf4ad2079096 159 break;
whutsup 4:bf4ad2079096 160
whutsup 4:bf4ad2079096 161
whutsup 4:bf4ad2079096 162 }
whutsup 0:cbbc3528eb59 163 }
whutsup 0:cbbc3528eb59 164
whutsup 2:b7d4195e0fea 165
whutsup 3:b79aa7d836fb 166 void OperateLED()
whutsup 3:b79aa7d836fb 167 {
whutsup 3:b79aa7d836fb 168 led =!led;
whutsup 3:b79aa7d836fb 169 }
whutsup 0:cbbc3528eb59 170
whutsup 4:bf4ad2079096 171 void ManualMk()
whutsup 4:bf4ad2079096 172 {
whutsup 4:bf4ad2079096 173
whutsup 4:bf4ad2079096 174 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 175 {
Bhoney 6:549177f76f8e 176 bt.puts("Please Wait Moving Anybaro"); //<STA>
whutsup 4:bf4ad2079096 177 }
whutsup 4:bf4ad2079096 178 else
whutsup 4:bf4ad2079096 179 {
whutsup 4:bf4ad2079096 180 sysStatus = 6;
whutsup 4:bf4ad2079096 181 MkActionManual();
whutsup 4:bf4ad2079096 182 }
whutsup 4:bf4ad2079096 183
whutsup 4:bf4ad2079096 184
whutsup 4:bf4ad2079096 185 }
whutsup 4:bf4ad2079096 186
whutsup 4:bf4ad2079096 187 void ButtonSys()
whutsup 4:bf4ad2079096 188 {
whutsup 4:bf4ad2079096 189 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 190 {
Bhoney 6:549177f76f8e 191 bt.puts("Please Wait Moving Anybaro"); //<STA> 또는 ifdef debug 처리?
Bhoney 6:549177f76f8e 192
whutsup 4:bf4ad2079096 193 }
whutsup 4:bf4ad2079096 194 else
whutsup 4:bf4ad2079096 195 {
whutsup 4:bf4ad2079096 196 MotorButton();
whutsup 4:bf4ad2079096 197 }
whutsup 4:bf4ad2079096 198 }
whutsup 4:bf4ad2079096 199
whutsup 0:cbbc3528eb59 200 int main()
whutsup 0:cbbc3528eb59 201 {
whutsup 4:bf4ad2079096 202 bt.baud(115200);
whutsup 8:a435e7aa7a02 203 // pc.baud(115200);
whutsup 10:3fcaf50f528f 204 bt.puts("START 190627 Ver.8 \n");
Bhoney 6:549177f76f8e 205 int modeNum = 0;
Bhoney 6:549177f76f8e 206 int modeMotor = 0;
whutsup 4:bf4ad2079096 207
whutsup 8:a435e7aa7a02 208 char tmpCommand[4]={0,}; // [ADD] command
whutsup 4:bf4ad2079096 209 int rxVal;
whutsup 0:cbbc3528eb59 210
whutsup 0:cbbc3528eb59 211 up.mode(PullNone);
whutsup 0:cbbc3528eb59 212 down.mode(PullNone);
whutsup 8:a435e7aa7a02 213 bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
whutsup 2:b7d4195e0fea 214
whutsup 4:bf4ad2079096 215 up.fall(&ButtonSys);
whutsup 4:bf4ad2079096 216 up.rise(&ButtonSys);
whutsup 4:bf4ad2079096 217 down.fall(&ButtonSys);
whutsup 4:bf4ad2079096 218 down.rise(&ButtonSys);
whutsup 0:cbbc3528eb59 219
whutsup 10:3fcaf50f528f 220 bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
whutsup 3:b79aa7d836fb 221
whutsup 9:7d6fa62f9022 222 timer0.attach(&OperateLED,0.2);
whutsup 10:3fcaf50f528f 223 // timer4.attach(&Test,0.1);
whutsup 4:bf4ad2079096 224 bt.attach(&ReadData, Serial::RxIrq);
whutsup 3:b79aa7d836fb 225
whutsup 3:b79aa7d836fb 226
whutsup 0:cbbc3528eb59 227 while(1)
whutsup 0:cbbc3528eb59 228 {
whutsup 4:bf4ad2079096 229
whutsup 4:bf4ad2079096 230 if (1 == flagRx)
whutsup 4:bf4ad2079096 231 {
whutsup 8:a435e7aa7a02 232
whutsup 4:bf4ad2079096 233 flagRx = 0;
whutsup 4:bf4ad2079096 234 tmpCommand[0] = rxData[0];
whutsup 4:bf4ad2079096 235 tmpCommand[1] = rxData[1];
whutsup 4:bf4ad2079096 236 tmpCommand[2] = rxData[2];
whutsup 4:bf4ad2079096 237 rxVal = atoi(rxData+3);
whutsup 0:cbbc3528eb59 238
whutsup 4:bf4ad2079096 239
whutsup 10:3fcaf50f528f 240 if (0 == strcmp(tmpCommand,"MOD"))
whutsup 4:bf4ad2079096 241 {
Bhoney 6:549177f76f8e 242 modeNum = rxVal; // 1 ~ 6
Bhoney 6:549177f76f8e 243 sysStatus = rxVal; // Why this value are assigned directly?
Bhoney 6:549177f76f8e 244 // assigning sys variable after checking condition is correct progress, i think.
whutsup 4:bf4ad2079096 245 ModeSelect(modeNum);
whutsup 4:bf4ad2079096 246 }
whutsup 0:cbbc3528eb59 247
whutsup 4:bf4ad2079096 248 else if (0 == strcmp(tmpCommand,"MOT"))
whutsup 10:3fcaf50f528f 249 {
whutsup 4:bf4ad2079096 250 modeMotor = rxVal;
Bhoney 6:549177f76f8e 251 MotorTest(modeMotor);
whutsup 1:a003b900b810 252 }
whutsup 1:a003b900b810 253
whutsup 4:bf4ad2079096 254 else if (0 == strcmp(tmpCommand,"POS"))
whutsup 4:bf4ad2079096 255 {
whutsup 8:a435e7aa7a02 256 // errorPrevious = 0;
whutsup 4:bf4ad2079096 257 targetDis = rxVal;
whutsup 8:a435e7aa7a02 258 // float d1 = disSensor.read();
whutsup 8:a435e7aa7a02 259 // float d2 = parA*d1*d1+parB*d1+parC;
whutsup 8:a435e7aa7a02 260 // errorPrevious = targetDis - d2;
whutsup 8:a435e7aa7a02 261
whutsup 8:a435e7aa7a02 262 timer1.attach(&ControlAng,0.01);
whutsup 4:bf4ad2079096 263 }
whutsup 4:bf4ad2079096 264
whutsup 4:bf4ad2079096 265 if (0 == strcmp(tmpCommand,"MIL"))
whutsup 1:a003b900b810 266 {
whutsup 4:bf4ad2079096 267 sysStatus = 5;
whutsup 4:bf4ad2079096 268 targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
whutsup 4:bf4ad2079096 269 milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
whutsup 4:bf4ad2079096 270 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 271
whutsup 4:bf4ad2079096 272 if ( 0 == mkOn) mkOn = 1;
whutsup 4:bf4ad2079096 273
whutsup 1:a003b900b810 274 }
whutsup 4:bf4ad2079096 275
whutsup 4:bf4ad2079096 276
whutsup 4:bf4ad2079096 277 }
whutsup 2:b7d4195e0fea 278
whutsup 0:cbbc3528eb59 279
whutsup 0:cbbc3528eb59 280 }
whutsup 0:cbbc3528eb59 281
whutsup 0:cbbc3528eb59 282 }