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

Committer:
whutsup
Date:
Wed Dec 26 10:58:58 2018 +0000
Revision:
5:6bb52b2a79bf
Parent:
4:bf4ad2079096
Child:
7:8fb5513a2231
updated 181226

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