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@4:bf4ad2079096, 2018-12-21 (annotated)

Committer:
whutsup
Date:
Fri Dec 21 20:39:24 2018 +0000
Revision:
4:bf4ad2079096
Parent:
3:b79aa7d836fb
Child:
5:6bb52b2a79bf
Child:
6:549177f76f8e
updated 181222

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
whutsup 0:cbbc3528eb59 94 break;
whutsup 0:cbbc3528eb59 95 }
whutsup 0:cbbc3528eb59 96
whutsup 0:cbbc3528eb59 97 }
whutsup 0:cbbc3528eb59 98
whutsup 4:bf4ad2079096 99 void stop()
whutsup 0:cbbc3528eb59 100 {
whutsup 4:bf4ad2079096 101 MotorTest(0); // [ADD] Stop
whutsup 4:bf4ad2079096 102 }
whutsup 0:cbbc3528eb59 103
whutsup 4:bf4ad2079096 104 void ReadAng()
whutsup 4:bf4ad2079096 105 {
whutsup 4:bf4ad2079096 106 float d1 = disSensor.read();
whutsup 4:bf4ad2079096 107 float d2 = parA*d1*d1+parB*d1+parC;
whutsup 4:bf4ad2079096 108 bt.printf("%1.2f",d2);
whutsup 0:cbbc3528eb59 109 }
whutsup 0:cbbc3528eb59 110
whutsup 4:bf4ad2079096 111 void ModeSelect(int mode) // [ADD] 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 1:a003b900b810 126
whutsup 4:bf4ad2079096 127 timer3.detach();
whutsup 4:bf4ad2079096 128 timer2.detach();
whutsup 4:bf4ad2079096 129 timer1.detach();
whutsup 4:bf4ad2079096 130 onewayNum = 0;
whutsup 4:bf4ad2079096 131 milLoop = 0;
whutsup 4:bf4ad2079096 132
whutsup 4:bf4ad2079096 133 MotorTest(2);
whutsup 4:bf4ad2079096 134 ResetTimer.attach(&stop,10);
whutsup 4:bf4ad2079096 135
whutsup 4:bf4ad2079096 136 break;
whutsup 4:bf4ad2079096 137
whutsup 4:bf4ad2079096 138 case 2 : //Read Force
whutsup 1:a003b900b810 139
whutsup 4:bf4ad2079096 140 timer2.attach(&CalForce,0.1);
whutsup 4:bf4ad2079096 141
whutsup 4:bf4ad2079096 142 break;
whutsup 4:bf4ad2079096 143
whutsup 4:bf4ad2079096 144 case 3 : //Read Angle
whutsup 1:a003b900b810 145
whutsup 4:bf4ad2079096 146 timer2.detach();
whutsup 4:bf4ad2079096 147 ReadAng();
whutsup 4:bf4ad2079096 148
whutsup 4:bf4ad2079096 149 break;
whutsup 4:bf4ad2079096 150
whutsup 4:bf4ad2079096 151 case 4 : //Pause Temporarily
whutsup 4:bf4ad2079096 152
whutsup 4:bf4ad2079096 153 MotorTest(0);
whutsup 4:bf4ad2079096 154
whutsup 4:bf4ad2079096 155 break;
whutsup 4:bf4ad2079096 156
whutsup 4:bf4ad2079096 157
whutsup 4:bf4ad2079096 158 }
whutsup 0:cbbc3528eb59 159 }
whutsup 0:cbbc3528eb59 160
whutsup 2:b7d4195e0fea 161
whutsup 3:b79aa7d836fb 162 void OperateLED()
whutsup 3:b79aa7d836fb 163 {
whutsup 3:b79aa7d836fb 164 led =!led;
whutsup 3:b79aa7d836fb 165 }
whutsup 0:cbbc3528eb59 166
whutsup 4:bf4ad2079096 167 void ManualMk()
whutsup 4:bf4ad2079096 168 {
whutsup 4:bf4ad2079096 169
whutsup 4:bf4ad2079096 170 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 171 {
whutsup 4:bf4ad2079096 172 bt.puts("Please Wait Moving Anybaro");
whutsup 4:bf4ad2079096 173 }
whutsup 4:bf4ad2079096 174 else
whutsup 4:bf4ad2079096 175 {
whutsup 4:bf4ad2079096 176 sysStatus = 6;
whutsup 4:bf4ad2079096 177 MkActionManual();
whutsup 4:bf4ad2079096 178 }
whutsup 4:bf4ad2079096 179
whutsup 4:bf4ad2079096 180
whutsup 4:bf4ad2079096 181 }
whutsup 4:bf4ad2079096 182
whutsup 4:bf4ad2079096 183 void ButtonSys()
whutsup 4:bf4ad2079096 184 {
whutsup 4:bf4ad2079096 185 if( 1 == sysStatus | 2 == sysStatus)
whutsup 4:bf4ad2079096 186 {
whutsup 4:bf4ad2079096 187 bt.puts("Please Wait Moving Anybaro");
whutsup 4:bf4ad2079096 188 }
whutsup 4:bf4ad2079096 189 else
whutsup 4:bf4ad2079096 190 {
whutsup 4:bf4ad2079096 191 MotorButton();
whutsup 4:bf4ad2079096 192 }
whutsup 4:bf4ad2079096 193 }
whutsup 4:bf4ad2079096 194
whutsup 0:cbbc3528eb59 195 int main()
whutsup 0:cbbc3528eb59 196 {
whutsup 4:bf4ad2079096 197 bt.baud(115200);
whutsup 4:bf4ad2079096 198 pc.baud(115200);
whutsup 4:bf4ad2079096 199 bt.puts("START 181203 Ver.6 \n");
whutsup 4:bf4ad2079096 200
whutsup 4:bf4ad2079096 201 int modeNum = 0;
whutsup 4:bf4ad2079096 202 int modeLED = 0;
whutsup 4:bf4ad2079096 203 int modeMotor =0;
whutsup 4:bf4ad2079096 204
whutsup 4:bf4ad2079096 205 char tmpCommand[4]={0,}; // [ADD] command
whutsup 4:bf4ad2079096 206 int rxVal;
whutsup 0:cbbc3528eb59 207
whutsup 0:cbbc3528eb59 208 up.mode(PullNone);
whutsup 0:cbbc3528eb59 209 down.mode(PullNone);
whutsup 4:bf4ad2079096 210 bt_ml.mode(PullNone); // [ADD] Setup Millking Action Manual Button
whutsup 2:b7d4195e0fea 211
whutsup 4:bf4ad2079096 212 up.fall(&ButtonSys);
whutsup 4:bf4ad2079096 213 up.rise(&ButtonSys);
whutsup 4:bf4ad2079096 214 down.fall(&ButtonSys);
whutsup 4:bf4ad2079096 215 down.rise(&ButtonSys);
whutsup 0:cbbc3528eb59 216
whutsup 4:bf4ad2079096 217 bt_ml.fall(&ManualMk); // [ADD] Interrupt Millking Action Manual Button
whutsup 3:b79aa7d836fb 218
whutsup 4:bf4ad2079096 219 timer0.attach(&OperateLED,0.2);
whutsup 0:cbbc3528eb59 220
whutsup 4:bf4ad2079096 221 bt.attach(&ReadData, Serial::RxIrq);
whutsup 3:b79aa7d836fb 222
whutsup 3:b79aa7d836fb 223
whutsup 0:cbbc3528eb59 224 while(1)
whutsup 0:cbbc3528eb59 225 {
whutsup 4:bf4ad2079096 226
whutsup 4:bf4ad2079096 227 if (1 == flagRx)
whutsup 4:bf4ad2079096 228 {
whutsup 4:bf4ad2079096 229 flagRx = 0;
whutsup 4:bf4ad2079096 230 tmpCommand[0] = rxData[0];
whutsup 4:bf4ad2079096 231 tmpCommand[1] = rxData[1];
whutsup 4:bf4ad2079096 232 tmpCommand[2] = rxData[2];
whutsup 4:bf4ad2079096 233 rxVal = atoi(rxData+3);
whutsup 0:cbbc3528eb59 234
whutsup 4:bf4ad2079096 235 if (0 == strcmp(tmpCommand,"LED"))
whutsup 0:cbbc3528eb59 236 {
whutsup 4:bf4ad2079096 237 #ifdef DEBUG
whutsup 4:bf4ad2079096 238 bt.puts("\nLED CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 239 #endif
whutsup 4:bf4ad2079096 240 modeLED = rxVal;
whutsup 0:cbbc3528eb59 241 ControlLED(modeLED);
whutsup 0:cbbc3528eb59 242 }
whutsup 4:bf4ad2079096 243
whutsup 4:bf4ad2079096 244 else if (0 == strcmp(tmpCommand,"MOD"))
whutsup 4:bf4ad2079096 245 {
whutsup 4:bf4ad2079096 246 #ifdef DEBUG
whutsup 4:bf4ad2079096 247 bt.puts("\nMODE SELECT!!\n");
whutsup 4:bf4ad2079096 248 #endif
whutsup 4:bf4ad2079096 249 modeNum = rxVal;
whutsup 4:bf4ad2079096 250 sysStatus = rxVal;
whutsup 4:bf4ad2079096 251 ModeSelect(modeNum);
whutsup 4:bf4ad2079096 252 }
whutsup 0:cbbc3528eb59 253
whutsup 4:bf4ad2079096 254 else if (0 == strcmp(tmpCommand,"MOT"))
whutsup 4:bf4ad2079096 255 {
whutsup 4:bf4ad2079096 256 #ifdef DEBUG
whutsup 4:bf4ad2079096 257 bt.puts("\nMOTOR TEST CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 258 #endif
whutsup 4:bf4ad2079096 259 modeMotor = rxVal;
whutsup 4:bf4ad2079096 260 MotorTest(modeMotor);
whutsup 1:a003b900b810 261 }
whutsup 1:a003b900b810 262
whutsup 4:bf4ad2079096 263 else if (0 == strcmp(tmpCommand,"POS"))
whutsup 4:bf4ad2079096 264 {
whutsup 4:bf4ad2079096 265 #ifdef DEBUG
whutsup 4:bf4ad2079096 266 bt.puts("\nMOTOR DISTANCE CONTROL MODE!!\n");
whutsup 4:bf4ad2079096 267 #endif
whutsup 4:bf4ad2079096 268 targetDis = rxVal;
whutsup 4:bf4ad2079096 269 timer1.attach(&ControlAng,0.8);
whutsup 4:bf4ad2079096 270 }
whutsup 4:bf4ad2079096 271
whutsup 4:bf4ad2079096 272 if (0 == strcmp(tmpCommand,"MIL"))
whutsup 1:a003b900b810 273 {
whutsup 4:bf4ad2079096 274 sysStatus = 5;
whutsup 4:bf4ad2079096 275 #ifdef DEBUG
whutsup 4:bf4ad2079096 276 bt.puts("\nMILLKING ACTION MODE!!\n");
whutsup 4:bf4ad2079096 277 #endif
whutsup 4:bf4ad2079096 278 targetDis = atoi(rxData+5); // [ADD] return 2 characters in the back of 4 characters
whutsup 4:bf4ad2079096 279 milLoop = 0.01*(atoi(rxData+3)-atoi(rxData+5)); // [ADD] return 2 cahracters in the front of 4 characters
whutsup 4:bf4ad2079096 280 timer3.attach(&MkAction,0.1);
whutsup 4:bf4ad2079096 281
whutsup 4:bf4ad2079096 282 if ( 0 == mkOn) mkOn = 1;
whutsup 4:bf4ad2079096 283
whutsup 1:a003b900b810 284 }
whutsup 4:bf4ad2079096 285
whutsup 4:bf4ad2079096 286
whutsup 4:bf4ad2079096 287 }
whutsup 2:b7d4195e0fea 288
whutsup 0:cbbc3528eb59 289
whutsup 0:cbbc3528eb59 290 }
whutsup 0:cbbc3528eb59 291
whutsup 0:cbbc3528eb59 292 }