Diff: main.cpp

Revision:

3:759af41b06ee

Parent:

2:399bcafe6f36

Child:

4:000065db6ae4

--- a/main.cpp	Fri Mar 26 17:03:52 2021 +0000
+++ b/main.cpp	Sun Apr 25 06:25:21 2021 +0000
@@ -1,10 +1,29 @@
 #include "mbed.h"
 #include "Servo.h"
+#include "QEI.h"
 
 #include <ros.h>
 #include "std_msgs/Bool.h"
 #include "geometry_msgs/Twist.h"
 
+
+
+#define GET_LOW_char(A) (uint8_t)((A))
+//Macro function  get lower 8 bits of A
+#define GET_HIGH_char(A) (uint8_t)((A) >> 8)
+//Macro function  get higher 8 bits of A
+#define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
+//put A as higher 8 bits   B as lower 8 bits   which amalgamated into 16 bits integer
+
+#define ID0 0
+#define ID1 1
+#define ID2 2
+#define ID3 3
+#define ID4 4
+
+#define LOBOT_SERVO_FRAME_HEADER         0x55
+#define LOBOT_SERVO_MOVE_TIME_WRITE      1
+
 #define BAUD_RATE 115200
 
 using namespace std;
@@ -12,58 +31,149 @@
 using namespace ros;
 using namespace geometry_msgs;
 
-DigitalOut myled = LED1;
-DigitalOut Refer_Volt_3_3 = PB_1;
-DigitalOut pin_A = PA_10;
-DigitalOut pin_B = PA_2;
-DigitalOut pin_C = PA_3;
+DigitalOut out1(D2);    //  Motor direction control pin 1
+DigitalOut out2(D4);    //  Motor direction control pin 2
+DigitalIn home_button(D13);     // Button for setting the home button
 
 
+DigitalOut myled = LED1;
+DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
+DigitalOut pin_A = D12;         //not in use
+DigitalOut pin_B = D11;
+DigitalOut pin_C = D10;
 
-NodeHandle nh;
+BufferedSerial serial_port(D1, D0, 16, 16); // tx, rx, buffer size, multiple
+
+NodeHandle nh;      //ROS node
 
 Servo servo = D6;
 
-void messagesqu(const Bool& _msg){
+QEI dc_motor (D8,D7,NC,792);    // Create QEI object for increment encoder
+int counter = 0;    // Dummy counter for sample program
+
+void reset(){       // Setting home point for increment encoder for stepper motor
+    while (home_button == 0){       // Continue to turn clockwise until home button pressed
+        out1 = 1;
+        out2 = 0;
+    }
+    out1 = 0;
+    dc_motor.reset();       //  Reset pulse_
+    wait(1);
+ };
+ 
+ void go_angle(int angle){      //  Move motor to specific angle from home point
+    int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0);    //  Calculating target pulse number
+    int cur_pulse = dc_motor.getPulses();
+    
+    while ( tar_pulse != cur_pulse ){
+
+        if (tar_pulse > cur_pulse){     //  Rotate motor clockwise
+            out1 = 1;
+            out2 = 0;
+        }
+        else {                          //  Rotate motor counter clockwrise 
+            out1 = 0;
+            out2 = 1;
+        }
+        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
+    }
+    out1 = 0;       //  Stop motor
+    out2 = 0;
+};
+
+void LobotSerialoneServoMove(uint8_t id, int16_t position, uint16_t time)//robot arm move dedicated position
+{
+  char buf[10];
+  if(position < 0)
+    position = 0;
+  if(position > 1000)
+    position = 1000;
+  buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
+  buf[2] = 0x08;
+  buf[3] = 0x03;
+  buf[4] = 0x01;
+  buf[5] = GET_LOW_char(time);
+  buf[6] = GET_HIGH_char(time);
+  buf[7] = id;
+  buf[8] = GET_LOW_char(position);
+  buf[9] = GET_HIGH_char(position);
+  serial_port.write(buf, 10);
+}
+
+void SerialMoveActionGroup(uint8_t gpid, uint16_t times)//robot arm move action group(No. action group, repeat time)
+{
+    
+    char buf[7];
+    buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
+    buf[2]=0x05;
+    buf[3]=0x06;
+    buf[4]=gpid;
+    buf[5]=GET_LOW_char(times);
+    buf[6]=GET_HIGH_char(times);
+    serial_port.write(buf,7);
+    
+}
+void messagesqu(const Bool& _msg){  //callback of square
     bool check = _msg.data;
     if(check)
-        pin_A = 1;
-    else
-        pin_A = 0;
+        //SerialMoveActionGroup(ID1,1);
+        SerialMoveActionGroup(ID0,1);
 }
 
-void messagecro(const Bool& _msg){
+void messagecro(const Bool& _msg){  //callback of cross
     bool check = _msg.data;
     if(check)
-        pin_B = 1;
-    else
-        pin_B = 0;
+        //SerialMoveActionGroup(ID1,1);
+        SerialMoveActionGroup(ID1,1);
 }
 
 
-void messagecir(const Bool& _msg){
+void messagecir(const Bool& _msg){  //callback of circle
     bool check = _msg.data;
-    if(check && servo == 0) {
-        servo = 1.0;
-    }
-    else if(check && servo == 1)
-    {
-        servo = 0; 
-    }
+    if(check)
+        //SerialMoveActionGroup(ID1,1);
+        SerialMoveActionGroup(ID2,1);
+//if(check && servo == 0) {
+//        servo = 1.0;
+//    }
+//    else if(check && servo == 1)
+//    {
+//        servo = 0; 
+//    }
+}
+
+void messagetri(const Bool& _msg){  //callback of triangle
+    bool check = _msg.data;
+    if(check)
+        //SerialMoveActionGroup(ID1,1);
+        SerialMoveActionGroup(ID3,1);
 }
 
-void messagetri(const Bool& _msg){
-    bool check = _msg.data;
-    if(check)
-        pin_C = 1;
-    else
-        pin_C = 0;
-}
-
-void messagekeypad (const Twist& _msg){
+void messagekeypad (const Twist& _msg){     //callback of keypad
     float x = _msg.linear.x;
     float y = _msg.linear.y;
-    
+    if (x==1){                                 //  Rotate motor clockwise
+        out1 = 1;
+        out2 = 0;
+    }
+    else if(x==-1) {                          //  Rotate motor anti-clockwrise 
+        out1 = 0;
+        out2 = 1;
+    }
+    else{
+        out1 = 0;
+        out2 = 0;
+    }
+    if (y==1){                                  //  pump air of pnumetic part
+        pin_B = 1;
+    }
+    else if(y==-1) {                          //  release air of pnumetic part
+        pin_C = 1;
+    }
+    else{
+        pin_B = 0;
+        pin_C = 0;
+    }
 }
 
 Subscriber<Bool> subsqu("button_square", &messagesqu);
@@ -76,17 +186,16 @@
     myled = 0;
     servo = 0;
     
-    nh.getHardware()->setBaud(BAUD_RATE);
+    nh.getHardware()->setBaud(BAUD_RATE);   //set baud rate
     nh.initNode();
     
     nh.subscribe(subsqu);
     nh.subscribe(subcro);
     nh.subscribe(subcir);
     nh.subscribe(subtri);
-    
+    nh.subscribe(subkeypad);
     while(true) {
         nh.spinOnce();
-        
         if(nh.connected())
             myled = 1;
         else