DECS @UNIST
/
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
Diff: main.cpp
- Revision:
- 0:cbbc3528eb59
- Child:
- 1:a003b900b810
diff -r 000000000000 -r cbbc3528eb59 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jun 22 12:16:52 2018 +0000
@@ -0,0 +1,259 @@
+#include "mbed.h"
+
+Serial bc(PA_9,PA_10); // USART1 Bluetooth (Tx, Rx)
+Serial pc(PA_2,PA_3); // USART2 pc (Tx, Rx)
+
+DigitalOut led(LED2); // LED
+DigitalOut enable(PB_12); // Motor enable
+DigitalOut p1(PB_13); // Motor dir1 up
+DigitalOut p2(PB_14); // Motor dir2 down
+
+DigitalIn up(PC_8); // Burton Motor control
+DigitalIn down(PC_9);
+
+AnalogIn disSensor(PA_7); // Distance Sensor
+
+Ticker timer1; //
+Ticker timer2; //
+
+/*------------------ ReadData Variables -------------------*/
+
+uint8_t rx_buf[8]; // < _ _ _ _ _ _ > 8Byte
+char RX_command[4] = {0,}; // _ _ _
+char RX_data[4] = {0,}; // _ _ _
+char flag_RX = 0;
+event_callback_t Test;
+
+int targetDis;
+
+/*------------------ Callback Functions -------------------*/
+
+void ReadData(int events)
+{
+ bc.printf("Read The Data!!\n");
+
+
+ if (rx_buf[0] == '<' && rx_buf[7] == '>')
+ {
+ flag_RX = 1;
+
+ RX_command[0] = rx_buf[1];
+ RX_command[1] = rx_buf[2];
+ RX_command[2] = rx_buf[3];
+ RX_data[0] = rx_buf[4];
+ RX_data[1] = rx_buf[5];
+ RX_data[2] = rx_buf[6];
+ }
+
+
+}
+
+void ControlLED(int mode)
+{
+ bc.printf("mode = %d\n",mode);
+
+ switch (mode)
+ {
+ case 0 :
+
+ led = 0;
+
+ break;
+
+ case 1 :
+ led = 1;
+
+ break;
+
+ case 2 :
+ bc.printf("ANYBARO");
+
+ break;
+
+ }
+
+}
+
+
+void MotorTest(int mode)
+{
+
+ bc.printf("mode = %d\n",mode);
+
+ switch (mode)
+
+ {
+ case 0 : //stop
+
+ enable = 0;
+
+ break;
+
+ case 1 : // up
+
+ enable = 1;
+ p1 = 0;
+ p2 = 1;
+
+ break;
+
+ case 2 : // down
+
+ enable = 1;
+ p1 = 1;
+ p2 = 0;
+
+ break;
+
+ }
+
+}
+
+void ControlAng()
+{
+ double d1 = disSensor.read();
+ double d2 = 161.89*d1*d1*d1*d1-437.31*d1*d1*d1+440.89*d1*d1-207.4*d1+42.245;
+
+ bc.printf("%1.3f\n", d1);
+
+// int ref_d = targetDis-d2;
+//
+// if(ref_d > 0.01)
+// {
+// enable = 1;
+// p1 = 1;
+// p2 = 0;
+//
+// }
+//
+// else if(ref_d < -0.01)
+// {
+//
+// enable = 1;
+// p1 = 0;
+// p2 = 1;
+//
+// }
+//
+// else
+// {
+//
+// enable = 0;
+// timer1.detach();
+//
+// }
+}
+
+void MotorBurton()
+{
+
+ int a = up.read();
+ int b = down.read();
+ int c = a*2 + b;
+
+ bc.printf("%d %d %d\n ",a,b,c);
+
+ switch(c)
+ {
+ case 0 : //stop
+
+ enable = 0;
+
+ break;
+
+ case 1 : // down
+
+ enable = 1;
+ p1 = 0;
+ p2 = 1;
+
+ break;
+
+ case 2 : // up
+
+ enable = 1;
+ p1 = 1;
+ p2 = 0;
+
+ break;
+
+ case 3 : // stop
+
+ enable = 1;
+ p1 = 0;
+ p2 = 0;
+
+ break;
+
+ }
+
+
+
+}
+
+
+int main()
+{
+ bc.baud(9600);
+ bc.printf("START 180622 Ver. \n");
+
+ up.mode(PullNone);
+ down.mode(PullNone);
+
+ int modeLED;
+ int modeMotor;
+
+ modeLED = 0;
+ modeMotor = 0;
+
+ timer2.attach(&MotorBurton,0.1);
+
+ while(1)
+ {
+
+ bc.read(rx_buf, 8, ReadData , SERIAL_EVENT_RX_COMPLETE, '\n');
+
+ // Readdata callback // int event = SERIAL_EVENT_RX_COMPLETE //
+
+ // analyze the recieved packets
+
+ if (1 == flag_RX)
+ {
+ flag_RX = 0;
+
+ if (0 == strcmp(RX_command,"LED"))
+ {
+ bc.printf("LED CONTROL MODE!!");
+ modeLED = atoi(RX_data);
+ ControlLED(modeLED);
+ }
+
+ else if (0 == strcmp(RX_command,"MOT"))
+ {
+
+ bc.printf("MOTOR CONTROL MODE!!");
+ modeMotor = atoi(RX_data);
+ MotorTest(modeMotor);
+ timer1.attach(&ControlAng,0.1);
+
+ }
+
+
+
+// else if (0 == strcmp(RX_command,"DIS"))
+// {
+//
+// bc.printf("MOTOR DISTANCE CONTROL MODE!!");
+// targetDis = atoi(RX_data);
+// timer1.attach(&ControlAng,0.1);
+//
+// }
+
+
+ }
+
+ wait_ms(100);
+
+ }
+
+}
\ No newline at end of file