Revision 2:55f39e7883a6, committed 2017-07-14

Comitter:

JessicaGao

Date:

Fri Jul 14 12:32:18 2017 +0000

Parent:

1:3ff94f82fddd

Commit message:

lighthouse;

Changed in this revision

diff -r 3ff94f82fddd -r 55f39e7883a6 main.cpp
--- a/main.cpp	Fri Feb 17 00:00:55 2017 +0000
+++ b/main.cpp	Fri Jul 14 12:32:18 2017 +0000
@@ -1,41 +1,76 @@
 #include "robotarm.h"
+#include <math.h>
+#include "serial.h"
+#define ON 1
+#define OFF 0
 
 Robotarm arm;
+DigitalOut beacon(p15);
+SerialControl serial;
+
 
 int main()
-{
-    wait(1); // Useful if you need to connect H-Term etc.
+{   
+    
+    int base_degree = 90;    
+    int flag = 1;
+    
     //Run the main initialisation routine
-    arm.init();
+    arm.init();  
+    serial.setup_serial_interfaces();
+    /* Set all servos speed to 0.1 */
+    servo.SetCRSpeed( BASE , 0.1 );
+    servo.SetCRSpeed( SHOULDER , 0.1 );
+    servo.SetCRSpeed( ELBOW , 0.1 );
+    
     //Reset the servos to center position (after 1 second delay)
     //NB This activates the servos (makes rigid) so be careful when using
     arm.zero_servos(1);
     //Wait till servos are zeroed
     wait(3);
-    //Initialise remote control
-    if(REMOTE_ENABLED == 1)remote.init();
-    
-    //User code can now go in a loop:
-    while(1) {
-        //Eg set all servos to 1948 then 2148
-          servo.SetGoal(BASE,1948,1);
-          servo.SetGoal(SHOULDER,1948,1);
-          servo.SetGoal(ELBOW,1948,1);
-          servo.SetGoal(WRIST,400,1);
-            //If you want to show detailed info about a servo over serial, use the following:
-            //servo.DebugData(WRIST);
-          wait(0.5);
-          servo.SetGoal(BASE,2148,1);
-          servo.SetGoal(SHOULDER,2148,1);
-          servo.SetGoal(ELBOW,2148,1);
-          servo.SetGoal(WRIST,600,1);
-          wait(0.5);
-            //Alternatively we can set all the servos then use trigger - observe the difference...
-            //servo.SetGoal(BASE,2148,0);
-            //servo.SetGoal(SHOULDER,2148,0);
-            //servo.SetGoal(ELBOW,2148,0);
-            //servo.SetGoal(WRIST,600,0);
-            //servo.trigger();
-            //wait(0.5);
+    display.clear_display();
+    display.set_position(0,0);
+    display.write_string("start"); 
+    servo.SetGoalDegrees( BASE , 0 , 1);
+    servo.SetGoalDegrees( SHOULDER , 0 , 1 );
+    servo.SetGoalDegrees( ELBOW , 90 , 1 );
+    while(1){
+        if(turning == 1){
+            beacon = ON;
+            if(flag == 1){
+                if(base_degree >= 180){
+                   flag = 0;
+                }
+                else {
+                    base_degree += 5;
+                }
+            }
+            else if(flag == 0){
+                if(base_degree <= 0){
+                    flag = 1;
+                }
+                else {
+                    base_degree -= 5;
+                }
+            }
+            servo.SetGoalDegrees( BASE , base_degree-90 , 1);
+        }
+        else if(turning == 0){
+           // turning = 3;
+            beacon = ON;
+            servo.SetGoalDegrees( BASE , base_degree - 90 , 1);
+            pc.printf("base_degree = %d \n", base_degree - 90);
+        }
+        else if(turning == 2){
+            beacon = OFF;
+            servo.SetGoalDegrees( BASE , 0 , 1);
+            base_degree = 90;
+            wait(0.5);
+        }
     }
-}
\ No newline at end of file
+  
+}
+
+   
+
+

diff -r 3ff94f82fddd -r 55f39e7883a6 serial.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/serial.cpp	Fri Jul 14 12:32:18 2017 +0000
@@ -0,0 +1,173 @@
+
+#include "serial.h"
+#include "mbed.h"
+#include "robotarm.h"
+
+#define ENABLE_BLUETOOTH 1
+#define ENABLE_PC_SERIAL 1
+#define COMMAND_MESSAGE_BYTE 0X1D
+
+#define PC_BAUD 115200
+#define BLUETOOTH_BAUD 115200
+
+Serial bt(p13, p14);
+
+int turning = 1;
+
+char pc_command_message_started = 0;
+char pc_command_message_byte = 0;
+char pc_command_message[3];
+
+char bt_command_message_started = 0;
+char bt_command_message_byte = 0;
+char bt_command_message[3];
+
+char file_transfer_mode = 0;
+Timeout pc_command_timeout;
+Timeout bt_command_timeout;
+
+void SerialControl::setup_serial_interfaces()
+{
+    if(ENABLE_PC_SERIAL) {
+        pc.baud(PC_BAUD);
+        pc.attach(this,&SerialControl::IF_pc_rx_callback, Serial::RxIrq);
+    }
+    if(ENABLE_BLUETOOTH) {
+        bt.baud(BLUETOOTH_BAUD);
+        bt.attach(this,&SerialControl::IF_bt_rx_callback, Serial::RxIrq);
+    }
+}
+    
+void SerialControl::IF_handle_command_serial_message(char message[3], char interface){
+    if(message[0]==1){
+        turning = 1;
+        pc.printf("start turn(beacon on)%d\n",turning);
+    }
+    else if(message[1]==1){
+        pc.printf("stop turn(beacon on)%d\n", turning);
+        turning = 0;
+    }
+    else if(message[2]==1){
+        pc.printf("beacon off\n");
+        turning = 2;
+    }
+    else if(message[0] == 2){
+        bt.printf("%c%d",COMMAND_MESSAGE_BYTE, turning);
+        pc.printf("%c%d", COMMAND_MESSAGE_BYTE, turning);
+    }
+}
+
+
+Timeout bt_message_timeout;
+//static float bt_message_timeout_period = 0.001; // 1 millisecond
+char bt_buffer[255];
+int bt_buffer_index = 0;
+
+void SerialControl::IF_bt_message_timeout()
+{
+    char buffer[255];
+
+    sprintf(buffer, bt_buffer, bt_buffer_index);
+    buffer[bt_buffer_index] = 0;
+    if(file_transfer_mode == 1) {
+    } else {
+//    debug("BT message timeout: %s [%d chars]\n", buffer, bt_buffer_index);
+        if(bt_buffer_index == 5 && buffer[0] == COMMAND_MESSAGE_BYTE && buffer[4] == COMMAND_MESSAGE_BYTE) {
+            bt_command_message[0] = buffer[1];
+            bt_command_message[1] = buffer[2];
+            bt_command_message[2] = buffer[3];
+            IF_handle_command_serial_message(bt_command_message , 1);
+        } 
+    }
+    bt_buffer_index = 0;
+}
+
+
+void SerialControl::IF_bt_rx_callback()
+{
+    int count = 0;
+    char message_array[255];
+
+    wait_ms(500); // Wait 0.5ms to allow a complete message to arrive before atttempting to process it
+
+    while(bt.readable()) {
+        char tc = bt.getc();
+        message_array[count] = tc;
+        count ++;
+        if(bt_command_message_started == 1) {
+            if(bt_command_message_byte == 3) {
+                bt_command_timeout.detach();
+                if(tc == COMMAND_MESSAGE_BYTE) {
+                    // A complete command message succesfully received, call handler
+                    bt_command_message_started = 0;
+                    count = 0;
+                    IF_handle_command_serial_message(bt_command_message , 1);
+                } else {
+                    // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
+                    bt_command_message_started = 0;
+                    message_array[0] = COMMAND_MESSAGE_BYTE;
+                    message_array[1] = bt_command_message[0];
+                    message_array[2] = bt_command_message[1];
+                    message_array[3] = bt_command_message[2];
+                    message_array[4] = tc;
+                    count = 5;
+                }
+            } else {
+
+                bt_command_message[bt_command_message_byte] = tc;
+                bt_command_message_byte ++;
+            }
+        } else {
+            if(count == 1) {
+                if(tc == COMMAND_MESSAGE_BYTE) {
+                    bt_command_message_started = 1;
+                    bt_command_message_byte = 0;
+
+                }
+            }
+        }
+    }
+}
+
+void SerialControl::IF_pc_rx_callback()
+{
+    int count = 0;
+    char message_array[255];
+
+    while(pc.readable()) {
+        char tc = pc.getc();
+        message_array[count] = tc;
+        count ++;
+        if(pc_command_message_started == 1) {
+            if(pc_command_message_byte == 3) {
+                pc_command_timeout.detach();
+                if(tc == COMMAND_MESSAGE_BYTE) {
+                    // A complete command message succesfully received, call handler
+                    pc_command_message_started = 0;
+                    count = 0;
+                    IF_handle_command_serial_message(pc_command_message , 0);
+                } else {
+                    // Message is not a valid command message as 5th byte is not correct; treat whole message as a user message
+                    pc_command_message_started = 0;
+                    message_array[0] = COMMAND_MESSAGE_BYTE;
+                    message_array[1] = pc_command_message[0];
+                    message_array[2] = pc_command_message[1];
+                    message_array[3] = pc_command_message[2];
+                    message_array[4] = tc;
+                    count = 5;
+                }
+            } else {
+                pc_command_message[pc_command_message_byte] = tc;
+                pc_command_message_byte ++;
+            }
+        } else {
+            if(count == 1) {
+                if(tc == COMMAND_MESSAGE_BYTE) {
+                    pc_command_message_started = 1;
+                    pc_command_message_byte = 0;
+
+                }
+            }
+        }
+    }
+}
\ No newline at end of file

diff -r 3ff94f82fddd -r 55f39e7883a6 serial.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/serial.h	Fri Jul 14 12:32:18 2017 +0000
@@ -0,0 +1,26 @@
+#ifndef SERIAL_H
+#define SERIAL_H
+
+#include "mbed.h"
+class SerialControl
+{
+public:
+//void handle_user_serial_message(char * message, char length, char interface);
+
+/**
+ *  Sets the baud rates and enables the serial interfaces (PC and BT) as defined in the settings.h file
+ *  Attaches listeners to both the serial ports that trigger when a message is received
+ */
+void setup_serial_interfaces(void);
+
+private:
+void IF_handle_command_serial_message(char message [3], char interface);
+void IF_pc_rx_callback(void);
+void IF_bt_rx_callback(void);
+void IF_bt_message_timeout(void);
+
+};
+extern int turning;
+extern Serial bt;
+extern Serial pc;
+#endif
\ No newline at end of file