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@7:8fb5513a2231, 2018-12-26 (annotated)

Committer:
whutsup
Date:
Wed Dec 26 11:04:24 2018 +0000
Revision:
7:8fb5513a2231
Parent:
5:6bb52b2a79bf
Parent:
6:549177f76f8e
Child:
8:a435e7aa7a02
merged with BH's commit

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 4:bf4ad2079096 12 InterruptIn bt_ml(PC_10); // Millking action button
whutsup 4:bf4ad2079096 13
whutsup 4:bf4ad2079096 14 DigitalOut led(LED2); // 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 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 5:6bb52b2a79bf 37 #define parA 343.75f // parmeter of equation (values of distance sensor)
whutsup 5:6bb52b2a79bf 38 #define parB -1*508.08f
whutsup 5:6bb52b2a79bf 39 #define parC 166.67f
whutsup 0:cbbc3528eb59 40
whutsup 4:bf4ad2079096 41 /*----------------------- Callback Functions ------------------------*/
whutsup 0:cbbc3528eb59 42
whutsup 5:6bb52b2a79bf 43 void Test()
whutsup 5:6bb52b2a79bf 44 {
whutsup 5:6bb52b2a79bf 45 float d1 = disSensor.read();
whutsup 5:6bb52b2a79bf 46
whutsup 5:6bb52b2a79bf 47 bt.printf("%1.3f\n",d1);
whutsup 5:6bb52b2a79bf 48 }
whutsup 5:6bb52b2a79bf 49
whutsup 4:bf4ad2079096 50 void ReadData()
whutsup 0:cbbc3528eb59 51 {
whutsup 4:bf4ad2079096 52 char inChar;
whutsup 4:bf4ad2079096 53 static char rxCount = 0;
whutsup 4:bf4ad2079096 54 static char rxBuf[7];
whutsup 0:cbbc3528eb59 55
whutsup 4:bf4ad2079096 56 while(1 == bt.readable())
whutsup 0:cbbc3528eb59 57 {
whutsup 4:bf4ad2079096 58 inChar = bt.getc();
whutsup 4:bf4ad2079096 59
whutsup 4:bf4ad2079096 60 if('<' == inChar)
whutsup 4:bf4ad2079096 61 {
whutsup 4:bf4ad2079096 62 rxCount = 1;
whutsup 4:bf4ad2079096 63 }
whutsup 0:cbbc3528eb59 64
whutsup 4:bf4ad2079096 65 else if (rxCount > 0 && rxCount <8)
whutsup 4:bf4ad2079096 66 {
whutsup 4:bf4ad2079096 67 rxBuf[rxCount-1] = inChar;
whutsup 4:bf4ad2079096 68 rxCount++;
whutsup 4:bf4ad2079096 69 }
whutsup 0:cbbc3528eb59 70
whutsup 4:bf4ad2079096 71 else if ( 8 == rxCount && '>' == inChar)
whutsup 4:bf4ad2079096 72 {
whutsup 4:bf4ad2079096 73 rxCount = 0;
whutsup 4:bf4ad2079096 74 flagRx = 1;
whutsup 4:bf4ad2079096 75 memcpy(rxData, rxBuf, 7);
whutsup 4:bf4ad2079096 76 }
whutsup 4:bf4ad2079096 77 }
whutsup 0:cbbc3528eb59 78 }
whutsup 0:cbbc3528eb59 79
whutsup 4:bf4ad2079096 80
whutsup 0:cbbc3528eb59 81 void ControlLED(int mode)
whutsup 0:cbbc3528eb59 82 {
whutsup 4:bf4ad2079096 83
whutsup 0:cbbc3528eb59 84
whutsup 0:cbbc3528eb59 85 switch (mode)
whutsup 0:cbbc3528eb59 86 {
whutsup 0:cbbc3528eb59 87 case 0 :
whutsup 4:bf4ad2079096 88 timer0.detach();
whutsup 0:cbbc3528eb59 89 led = 0;
whutsup 0:cbbc3528eb59 90
whutsup 0:cbbc3528eb59 91 break;
whutsup 0:cbbc3528eb59 92
whutsup 0:cbbc3528eb59 93 case 1 :
whutsup 4:bf4ad2079096 94 timer0.detach();
whutsup 0:cbbc3528eb59 95 led = 1;
whutsup 0:cbbc3528eb59 96
whutsup 0:cbbc3528eb59 97 break;
whutsup 0:cbbc3528eb59 98
whutsup 0:cbbc3528eb59 99 case 2 :
whutsup 4:bf4ad2079096 100 bt.puts("ANYBARO");
whutsup 0:cbbc3528eb59 101
Bhoney 6:549177f76f8e 102 break;
Bhoney 6:549177f76f8e 103
Bhoney 6:549177f76f8e 104 default:
Bhoney 6:549177f76f8e 105 break;
whutsup 0:cbbc3528eb59 106 }
whutsup 0:cbbc3528eb59 107
whutsup 0:cbbc3528eb59 108 }
whutsup 0:cbbc3528eb59 109
whutsup 4:bf4ad2079096 110 void stop()
whutsup 0:cbbc3528eb59 111 {
whutsup 4:bf4ad2079096 112 MotorTest(0); // [ADD] Stop
whutsup 4:bf4ad2079096 113 }
whutsup 0:cbbc3528eb59 114
whutsup 4:bf4ad2079096 115 void ReadAng()
whutsup 4:bf4ad2079096 116 {
whutsup 4:bf4ad2079096 117 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 118 float d2 = parA*d1*d1+parB*d1+parC;
Bhoney 6:549177f76f8e 119
Bhoney 6:549177f76f8e 120 // original
Bhoney 6:549177f76f8e 121 // bt.printf("%1.2f",d2);
Bhoney 6:549177f76f8e 122
Bhoney 6:549177f76f8e 123 // fixed
Bhoney 6:549177f76f8e 124 bt.printf("<DIO%1.4f>",d2);
Bhoney 6:549177f76f8e 125
whutsup 0:cbbc3528eb59 126 }
whutsup 0:cbbc3528eb59 127
whutsup 4:bf4ad2079096 128 void ModeSelect(int mode) // [ADD] MODE
whutsup 0:cbbc3528eb59 129 {
whutsup 4:bf4ad2079096 130
whutsup 4:bf4ad2079096 131 switch(mode)
whutsup 4:bf4ad2079096 132 {
whutsup 1:a003b900b810 133
whutsup 4:bf4ad2079096 134 case 0 : //Standard Mode
whutsup 4:bf4ad2079096 135
whutsup 4:bf4ad2079096 136 timer1.detach();
whutsup 4:bf4ad2079096 137 timer2.detach();
whutsup 4:bf4ad2079096 138 timer3.detach();
whutsup 4:bf4ad2079096 139
whutsup 4:bf4ad2079096 140 break;
whutsup 4:bf4ad2079096 141
whutsup 4:bf4ad2079096 142 case 1 : //Reset Mode
whutsup 1:a003b900b810 143
whutsup 4:bf4ad2079096 144 timer3.detach();
whutsup 4:bf4ad2079096 145 timer2.detach();
whutsup 4:bf4ad2079096 146 timer1.detach();
whutsup 4:bf4ad2079096 147 onewayNum = 0;
whutsup 4:bf4ad2079096 148 milLoop = 0;
whutsup 4:bf4ad2079096 149
whutsup 4:bf4ad2079096 150 MotorTest(2);
whutsup 4:bf4ad2079096 151 ResetTimer.attach(&stop,10);
whutsup 4:bf4ad2079096 152
whutsup 4:bf4ad2079096 153 break;
whutsup 4:bf4ad2079096 154
whutsup 4:bf4ad2079096 155 case 2 : //Read Force
whutsup 1:a003b900b810 156
whutsup 5:6bb52b2a79bf 157 timer2.attach(&CalForce,0.05);
whutsup 4:bf4ad2079096 158
whutsup 4:bf4ad2079096 159 break;
whutsup 4:bf4ad2079096 160
whutsup 4:bf4ad2079096 161 case 3 : //Read Angle
whutsup 1:a003b900b810 162
whutsup 4:bf4ad2079096 163 timer2.detach();
Bhoney 6:549177f76f8e 164 ReadAng(); // return only one message
whutsup 4:bf4ad2079096 165
whutsup 4:bf4ad2079096 166 break;
whutsup 4:bf4ad2079096 167
whutsup 4:bf4ad2079096 168 case 4 : //Pause Temporarily
whutsup 4:bf4ad2079096 169
whutsup 4:bf4ad2079096 170 MotorTest(0);
whutsup 4:bf4ad2079096 171
whutsup 4:bf4ad2079096 172 break;
whutsup 4:bf4ad2079096 173
whutsup 4:bf4ad2079096 174
whutsup 4:bf4ad2079096 175 }
whutsup 0:cbbc3528eb59 176 }
whutsup 0:cbbc3528eb59 177
whutsup 2:b7d4195e0fea 178
whutsup 3:b79aa7d836fb 179 void OperateLED()
whutsup 3:b79aa7d836fb 180 {
whutsup 3:b79aa7d836fb 181 led =!led;
whutsup 3:b79aa7d836fb 182 }
whutsup 0:cbbc3528eb59 183
whutsup 4:bf4ad2079096 184 void ManualMk()
whutsup 4:bf4ad2079096 185 {
whutsup 4:bf4ad2079096 186
whutsup 4:bf4ad2079096 187 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 188 {
Bhoney 6:549177f76f8e 189 bt.puts("Please Wait Moving Anybaro"); //<STA>
whutsup 4:bf4ad2079096 190 }
whutsup 4:bf4ad2079096 191 else
whutsup 4:bf4ad2079096 192 {
whutsup 4:bf4ad2079096 193 sysStatus = 6;
whutsup 4:bf4ad2079096 194 MkActionManual();
whutsup 4:bf4ad2079096 195 }
whutsup 4:bf4ad2079096 196
whutsup 4:bf4ad2079096 197
whutsup 4:bf4ad2079096 198 }
whutsup 4:bf4ad2079096 199
whutsup 4:bf4ad2079096 200 void ButtonSys()
whutsup 4:bf4ad2079096 201 {
whutsup 4:bf4ad2079096 202 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 203 {
Bhoney 6:549177f76f8e 204 bt.puts("Please Wait Moving Anybaro"); //<STA> 또는 ifdef debug 처리?
Bhoney 6:549177f76f8e 205
whutsup 4:bf4ad2079096 206 }
whutsup 4:bf4ad2079096 207 else
whutsup 4:bf4ad2079096 208 {
whutsup 4:bf4ad2079096 209 MotorButton();
whutsup 4:bf4ad2079096 210 }
whutsup 4:bf4ad2079096 211 }
whutsup 4:bf4ad2079096 212
whutsup 0:cbbc3528eb59 213 int main()
whutsup 0:cbbc3528eb59 214 {
whutsup 4:bf4ad2079096 215 bt.baud(115200);
whutsup 4:bf4ad2079096 216 pc.baud(115200);
Bhoney 6:549177f76f8e 217 bt.puts("START 181203 Ver.6 \n"); // ifdef debug
whutsup 4:bf4ad2079096 218
Bhoney 6:549177f76f8e 219 int modeNum = 0;
whutsup 4:bf4ad2079096 220 int modeLED = 0;
Bhoney 6:549177f76f8e 221 int modeMotor = 0;
whutsup 4:bf4ad2079096 222
whutsup 4:bf4ad2079096 223 char tmpCommand[4]={0,}; // [ADD] command
whutsup 4:bf4ad2079096 224 int rxVal;
whutsup 0:cbbc3528eb59 225
whutsup 0:cbbc3528eb59 226 up.mode(PullNone);
whutsup 0:cbbc3528eb59 227 down.mode(PullNone);
whutsup 4:bf4ad2079096 228 bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
whutsup 2:b7d4195e0fea 229
whutsup 4:bf4ad2079096 230 up.fall(&ButtonSys);
whutsup 4:bf4ad2079096 231 up.rise(&ButtonSys);
whutsup 4:bf4ad2079096 232 down.fall(&ButtonSys);
whutsup 4:bf4ad2079096 233 down.rise(&ButtonSys);
whutsup 0:cbbc3528eb59 234
whutsup 4:bf4ad2079096 235 bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
whutsup 3:b79aa7d836fb 236
whutsup 4:bf4ad2079096 237 timer0.attach(&OperateLED,0.2);
whutsup 5:6bb52b2a79bf 238 // timer4.attach(&Test,0.1);
whutsup 4:bf4ad2079096 239 bt.attach(&ReadData, Serial::RxIrq);
whutsup 3:b79aa7d836fb 240
whutsup 3:b79aa7d836fb 241
whutsup 0:cbbc3528eb59 242 while(1)
whutsup 0:cbbc3528eb59 243 {
whutsup 4:bf4ad2079096 244
whutsup 4:bf4ad2079096 245 if (1 == flagRx)
whutsup 4:bf4ad2079096 246 {
whutsup 4:bf4ad2079096 247 flagRx = 0;
whutsup 4:bf4ad2079096 248 tmpCommand[0] = rxData[0];
whutsup 4:bf4ad2079096 249 tmpCommand[1] = rxData[1];
whutsup 4:bf4ad2079096 250 tmpCommand[2] = rxData[2];
whutsup 4:bf4ad2079096 251 rxVal = atoi(rxData+3);
whutsup 0:cbbc3528eb59 252
whutsup 4:bf4ad2079096 253 if (0 == strcmp(tmpCommand,"LED"))
whutsup 0:cbbc3528eb59 254 {
whutsup 4:bf4ad2079096 255 #ifdef DEBUG
whutsup 4:bf4ad2079096 256 bt.puts("\nLED CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 257 #endif
Bhoney 6:549177f76f8e 258 modeLED = rxVal; // if(modeLED != 0 || modeLED != 1 || modeLED != 2)
whutsup 0:cbbc3528eb59 259 ControlLED(modeLED);
whutsup 0:cbbc3528eb59 260 }
whutsup 4:bf4ad2079096 261
whutsup 4:bf4ad2079096 262 else if (0 == strcmp(tmpCommand,"MOD"))
whutsup 4:bf4ad2079096 263 {
whutsup 4:bf4ad2079096 264 #ifdef DEBUG
whutsup 4:bf4ad2079096 265 bt.puts("\nMODE SELECT!!\n");
whutsup 4:bf4ad2079096 266 #endif
Bhoney 6:549177f76f8e 267 modeNum = rxVal; // 1 ~ 6
Bhoney 6:549177f76f8e 268 sysStatus = rxVal; // Why this value are assigned directly?
Bhoney 6:549177f76f8e 269 // assigning sys variable after checking condition is correct progress, i think.
whutsup 4:bf4ad2079096 270 ModeSelect(modeNum);
whutsup 4:bf4ad2079096 271 }
whutsup 0:cbbc3528eb59 272
whutsup 4:bf4ad2079096 273 else if (0 == strcmp(tmpCommand,"MOT"))
whutsup 4:bf4ad2079096 274 {
whutsup 4:bf4ad2079096 275 #ifdef DEBUG
whutsup 4:bf4ad2079096 276 bt.puts("\nMOTOR TEST CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 277 #endif
whutsup 4:bf4ad2079096 278 modeMotor = rxVal;
Bhoney 6:549177f76f8e 279 MotorTest(modeMotor);
whutsup 1:a003b900b810 280 }
whutsup 1:a003b900b810 281
whutsup 4:bf4ad2079096 282 else if (0 == strcmp(tmpCommand,"POS"))
whutsup 4:bf4ad2079096 283 {
whutsup 4:bf4ad2079096 284 #ifdef DEBUG
whutsup 4:bf4ad2079096 285 bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
Bhoney 6:549177f76f8e 286 #endif
whutsup 4:bf4ad2079096 287 targetDis = rxVal;
whutsup 4:bf4ad2079096 288 timer1.attach(&ControlAng,0.8);
whutsup 4:bf4ad2079096 289 }
whutsup 4:bf4ad2079096 290
whutsup 4:bf4ad2079096 291 if (0 == strcmp(tmpCommand,"MIL"))
whutsup 1:a003b900b810 292 {
whutsup 4:bf4ad2079096 293 sysStatus = 5;
whutsup 4:bf4ad2079096 294 #ifdef DEBUG
whutsup 4:bf4ad2079096 295 bt.puts("\nMILLKING ACTION MODE!!\n");
whutsup 4:bf4ad2079096 296 #endif
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 }