Diff: main.cpp

Revision:

4:000065db6ae4

Parent:

3:759af41b06ee

Child:

6:46e625708076

--- a/main.cpp	Sun Apr 25 06:25:21 2021 +0000
+++ b/main.cpp	Wed May 19 09:52:01 2021 +0000
@@ -1,29 +1,13 @@
 #include "mbed.h"
 #include "Servo.h"
-#include "QEI.h"
+
+#include "LSCServo.h"
+#include "DC_Motor_Controller.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;
@@ -31,10 +15,7 @@
 using namespace ros;
 using namespace geometry_msgs;
 
-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
-
+DC_Motor_Controller motor(D2,D4,D8,D7,792); //out1 out2 in1 in2 ppr
 
 DigitalOut myled = LED1;
 DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
@@ -42,97 +23,29 @@
 DigitalOut pin_B = D11;
 DigitalOut pin_C = D10;
 
-BufferedSerial serial_port(D1, D0, 16, 16); // tx, rx, buffer size, multiple
+LSCServo robot_arm(D1, D0);
 
 NodeHandle nh;      //ROS node
 
 Servo servo = D6;
 
-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)
-        //SerialMoveActionGroup(ID1,1);
-        SerialMoveActionGroup(ID0,1);
+        robot_arm.LobotSerialActionGroupRun(0,1);
 }
 
 void messagecro(const Bool& _msg){  //callback of cross
     bool check = _msg.data;
     if(check)
-        //SerialMoveActionGroup(ID1,1);
-        SerialMoveActionGroup(ID1,1);
+        robot_arm.LobotSerialActionGroupRun(1,1);
 }
 
 
 void messagecir(const Bool& _msg){  //callback of circle
     bool check = _msg.data;
     if(check)
-        //SerialMoveActionGroup(ID1,1);
-        SerialMoveActionGroup(ID2,1);
+        robot_arm.LobotSerialActionGroupRun(2,1);
 //if(check && servo == 0) {
 //        servo = 1.0;
 //    }
@@ -145,24 +58,23 @@
 void messagetri(const Bool& _msg){  //callback of triangle
     bool check = _msg.data;
     if(check)
-        //SerialMoveActionGroup(ID1,1);
-        SerialMoveActionGroup(ID3,1);
+        robot_arm.LobotSerialActionGroupRun(3,1);
 }
 
 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;
+        motor.out1 = 1;
+        motor.out2 = 0;
     }
     else if(x==-1) {                          //  Rotate motor anti-clockwrise 
-        out1 = 0;
-        out2 = 1;
+        motor.out1 = 0;
+        motor.out2 = 1;
     }
     else{
-        out1 = 0;
-        out2 = 0;
+        motor.out1 = 0;
+        motor.out2 = 0;
     }
     if (y==1){                                  //  pump air of pnumetic part
         pin_B = 1;