pin1,2,3,servo
Dependencies: Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 4:000065db6ae4
- Parent:
- 3:759af41b06ee
- Child:
- 6:46e625708076
diff -r 759af41b06ee -r 000065db6ae4 main.cpp --- 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;