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@6:549177f76f8e, 2018-12-26 (annotated)

Committer:
Bhoney
Date:
Wed Dec 26 10:54:31 2018 +0000
Revision:
6:549177f76f8e
Parent:
4:bf4ad2079096
Child:
7:8fb5513a2231
comments are added, BT protocol is applied.

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