Robocon 2021 / ros_button_2021

pin1,2,3,servo

Dependencies:   Servo Test2Boards LSCServo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@3:759af41b06ee, 2021-04-25 (annotated)

Committer:
howanglam3
Date:
Sun Apr 25 06:25:21 2021 +0000
Revision:
3:759af41b06ee
Parent:
2:399bcafe6f36
Child:
4:000065db6ae4
after final presentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bensonsinsin998 0:858c61a9c2de 1 #include "mbed.h"
bensonsinsin998 0:858c61a9c2de 2 #include "Servo.h"
howanglam3 3:759af41b06ee 3 #include "QEI.h"
bensonsinsin998 0:858c61a9c2de 4
bensonsinsin998 0:858c61a9c2de 5 #include <ros.h>
bensonsinsin998 0:858c61a9c2de 6 #include "std_msgs/Bool.h"
howanglam3 2:399bcafe6f36 7 #include "geometry_msgs/Twist.h"
bensonsinsin998 0:858c61a9c2de 8
howanglam3 3:759af41b06ee 9
howanglam3 3:759af41b06ee 10
howanglam3 3:759af41b06ee 11 #define GET_LOW_char(A) (uint8_t)((A))
howanglam3 3:759af41b06ee 12 //Macro function get lower 8 bits of A
howanglam3 3:759af41b06ee 13 #define GET_HIGH_char(A) (uint8_t)((A) >> 8)
howanglam3 3:759af41b06ee 14 //Macro function get higher 8 bits of A
howanglam3 3:759af41b06ee 15 #define char_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
howanglam3 3:759af41b06ee 16 //put A as higher 8 bits B as lower 8 bits which amalgamated into 16 bits integer
howanglam3 3:759af41b06ee 17
howanglam3 3:759af41b06ee 18 #define ID0 0
howanglam3 3:759af41b06ee 19 #define ID1 1
howanglam3 3:759af41b06ee 20 #define ID2 2
howanglam3 3:759af41b06ee 21 #define ID3 3
howanglam3 3:759af41b06ee 22 #define ID4 4
howanglam3 3:759af41b06ee 23
howanglam3 3:759af41b06ee 24 #define LOBOT_SERVO_FRAME_HEADER 0x55
howanglam3 3:759af41b06ee 25 #define LOBOT_SERVO_MOVE_TIME_WRITE 1
howanglam3 3:759af41b06ee 26
bensonsinsin998 0:858c61a9c2de 27 #define BAUD_RATE 115200
bensonsinsin998 0:858c61a9c2de 28
bensonsinsin998 0:858c61a9c2de 29 using namespace std;
bensonsinsin998 0:858c61a9c2de 30 using namespace std_msgs;
bensonsinsin998 0:858c61a9c2de 31 using namespace ros;
howanglam3 2:399bcafe6f36 32 using namespace geometry_msgs;
bensonsinsin998 0:858c61a9c2de 33
howanglam3 3:759af41b06ee 34 DigitalOut out1(D2); // Motor direction control pin 1
howanglam3 3:759af41b06ee 35 DigitalOut out2(D4); // Motor direction control pin 2
howanglam3 3:759af41b06ee 36 DigitalIn home_button(D13); // Button for setting the home button
bensonsinsin998 0:858c61a9c2de 37
howanglam3 2:399bcafe6f36 38
howanglam3 3:759af41b06ee 39 DigitalOut myled = LED1;
howanglam3 3:759af41b06ee 40 DigitalOut Refer_Volt_3_3 = PB_1;//the left pin of D4
howanglam3 3:759af41b06ee 41 DigitalOut pin_A = D12; //not in use
howanglam3 3:759af41b06ee 42 DigitalOut pin_B = D11;
howanglam3 3:759af41b06ee 43 DigitalOut pin_C = D10;
howanglam3 2:399bcafe6f36 44
howanglam3 3:759af41b06ee 45 BufferedSerial serial_port(D1, D0, 16, 16); // tx, rx, buffer size, multiple
howanglam3 3:759af41b06ee 46
howanglam3 3:759af41b06ee 47 NodeHandle nh; //ROS node
bensonsinsin998 0:858c61a9c2de 48
bensonsinsin998 0:858c61a9c2de 49 Servo servo = D6;
bensonsinsin998 0:858c61a9c2de 50
howanglam3 3:759af41b06ee 51 QEI dc_motor (D8,D7,NC,792); // Create QEI object for increment encoder
howanglam3 3:759af41b06ee 52 int counter = 0; // Dummy counter for sample program
howanglam3 3:759af41b06ee 53
howanglam3 3:759af41b06ee 54 void reset(){ // Setting home point for increment encoder for stepper motor
howanglam3 3:759af41b06ee 55 while (home_button == 0){ // Continue to turn clockwise until home button pressed
howanglam3 3:759af41b06ee 56 out1 = 1;
howanglam3 3:759af41b06ee 57 out2 = 0;
howanglam3 3:759af41b06ee 58 }
howanglam3 3:759af41b06ee 59 out1 = 0;
howanglam3 3:759af41b06ee 60 dc_motor.reset(); // Reset pulse_
howanglam3 3:759af41b06ee 61 wait(1);
howanglam3 3:759af41b06ee 62 };
howanglam3 3:759af41b06ee 63
howanglam3 3:759af41b06ee 64 void go_angle(int angle){ // Move motor to specific angle from home point
howanglam3 3:759af41b06ee 65 int tar_pulse = ((double) angle / 360.0)* (2.0 * 792.0); // Calculating target pulse number
howanglam3 3:759af41b06ee 66 int cur_pulse = dc_motor.getPulses();
howanglam3 3:759af41b06ee 67
howanglam3 3:759af41b06ee 68 while ( tar_pulse != cur_pulse ){
howanglam3 3:759af41b06ee 69
howanglam3 3:759af41b06ee 70 if (tar_pulse > cur_pulse){ // Rotate motor clockwise
howanglam3 3:759af41b06ee 71 out1 = 1;
howanglam3 3:759af41b06ee 72 out2 = 0;
howanglam3 3:759af41b06ee 73 }
howanglam3 3:759af41b06ee 74 else { // Rotate motor counter clockwrise
howanglam3 3:759af41b06ee 75 out1 = 0;
howanglam3 3:759af41b06ee 76 out2 = 1;
howanglam3 3:759af41b06ee 77 }
howanglam3 3:759af41b06ee 78 cur_pulse = dc_motor.getPulses(); // Get increment encoder current pulse number
howanglam3 3:759af41b06ee 79 }
howanglam3 3:759af41b06ee 80 out1 = 0; // Stop motor
howanglam3 3:759af41b06ee 81 out2 = 0;
howanglam3 3:759af41b06ee 82 };
howanglam3 3:759af41b06ee 83
howanglam3 3:759af41b06ee 84 void LobotSerialoneServoMove(uint8_t id, int16_t position, uint16_t time)//robot arm move dedicated position
howanglam3 3:759af41b06ee 85 {
howanglam3 3:759af41b06ee 86 char buf[10];
howanglam3 3:759af41b06ee 87 if(position < 0)
howanglam3 3:759af41b06ee 88 position = 0;
howanglam3 3:759af41b06ee 89 if(position > 1000)
howanglam3 3:759af41b06ee 90 position = 1000;
howanglam3 3:759af41b06ee 91 buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
howanglam3 3:759af41b06ee 92 buf[2] = 0x08;
howanglam3 3:759af41b06ee 93 buf[3] = 0x03;
howanglam3 3:759af41b06ee 94 buf[4] = 0x01;
howanglam3 3:759af41b06ee 95 buf[5] = GET_LOW_char(time);
howanglam3 3:759af41b06ee 96 buf[6] = GET_HIGH_char(time);
howanglam3 3:759af41b06ee 97 buf[7] = id;
howanglam3 3:759af41b06ee 98 buf[8] = GET_LOW_char(position);
howanglam3 3:759af41b06ee 99 buf[9] = GET_HIGH_char(position);
howanglam3 3:759af41b06ee 100 serial_port.write(buf, 10);
howanglam3 3:759af41b06ee 101 }
howanglam3 3:759af41b06ee 102
howanglam3 3:759af41b06ee 103 void SerialMoveActionGroup(uint8_t gpid, uint16_t times)//robot arm move action group(No. action group, repeat time)
howanglam3 3:759af41b06ee 104 {
howanglam3 3:759af41b06ee 105
howanglam3 3:759af41b06ee 106 char buf[7];
howanglam3 3:759af41b06ee 107 buf[0]=buf[1]=LOBOT_SERVO_FRAME_HEADER;
howanglam3 3:759af41b06ee 108 buf[2]=0x05;
howanglam3 3:759af41b06ee 109 buf[3]=0x06;
howanglam3 3:759af41b06ee 110 buf[4]=gpid;
howanglam3 3:759af41b06ee 111 buf[5]=GET_LOW_char(times);
howanglam3 3:759af41b06ee 112 buf[6]=GET_HIGH_char(times);
howanglam3 3:759af41b06ee 113 serial_port.write(buf,7);
howanglam3 3:759af41b06ee 114
howanglam3 3:759af41b06ee 115 }
howanglam3 3:759af41b06ee 116 void messagesqu(const Bool& _msg){ //callback of square
bensonsinsin998 0:858c61a9c2de 117 bool check = _msg.data;
bensonsinsin998 0:858c61a9c2de 118 if(check)
howanglam3 3:759af41b06ee 119 //SerialMoveActionGroup(ID1,1);
howanglam3 3:759af41b06ee 120 SerialMoveActionGroup(ID0,1);
bensonsinsin998 0:858c61a9c2de 121 }
bensonsinsin998 0:858c61a9c2de 122
howanglam3 3:759af41b06ee 123 void messagecro(const Bool& _msg){ //callback of cross
bensonsinsin998 0:858c61a9c2de 124 bool check = _msg.data;
bensonsinsin998 0:858c61a9c2de 125 if(check)
howanglam3 3:759af41b06ee 126 //SerialMoveActionGroup(ID1,1);
howanglam3 3:759af41b06ee 127 SerialMoveActionGroup(ID1,1);
bensonsinsin998 0:858c61a9c2de 128 }
bensonsinsin998 0:858c61a9c2de 129
howanglam3 1:ac39b48026ca 130
howanglam3 3:759af41b06ee 131 void messagecir(const Bool& _msg){ //callback of circle
bensonsinsin998 0:858c61a9c2de 132 bool check = _msg.data;
howanglam3 3:759af41b06ee 133 if(check)
howanglam3 3:759af41b06ee 134 //SerialMoveActionGroup(ID1,1);
howanglam3 3:759af41b06ee 135 SerialMoveActionGroup(ID2,1);
howanglam3 3:759af41b06ee 136 //if(check && servo == 0) {
howanglam3 3:759af41b06ee 137 // servo = 1.0;
howanglam3 3:759af41b06ee 138 // }
howanglam3 3:759af41b06ee 139 // else if(check && servo == 1)
howanglam3 3:759af41b06ee 140 // {
howanglam3 3:759af41b06ee 141 // servo = 0;
howanglam3 3:759af41b06ee 142 // }
howanglam3 3:759af41b06ee 143 }
howanglam3 3:759af41b06ee 144
howanglam3 3:759af41b06ee 145 void messagetri(const Bool& _msg){ //callback of triangle
howanglam3 3:759af41b06ee 146 bool check = _msg.data;
howanglam3 3:759af41b06ee 147 if(check)
howanglam3 3:759af41b06ee 148 //SerialMoveActionGroup(ID1,1);
howanglam3 3:759af41b06ee 149 SerialMoveActionGroup(ID3,1);
bensonsinsin998 0:858c61a9c2de 150 }
bensonsinsin998 0:858c61a9c2de 151
howanglam3 3:759af41b06ee 152 void messagekeypad (const Twist& _msg){ //callback of keypad
howanglam3 2:399bcafe6f36 153 float x = _msg.linear.x;
howanglam3 2:399bcafe6f36 154 float y = _msg.linear.y;
howanglam3 3:759af41b06ee 155 if (x==1){ // Rotate motor clockwise
howanglam3 3:759af41b06ee 156 out1 = 1;
howanglam3 3:759af41b06ee 157 out2 = 0;
howanglam3 3:759af41b06ee 158 }
howanglam3 3:759af41b06ee 159 else if(x==-1) { // Rotate motor anti-clockwrise
howanglam3 3:759af41b06ee 160 out1 = 0;
howanglam3 3:759af41b06ee 161 out2 = 1;
howanglam3 3:759af41b06ee 162 }
howanglam3 3:759af41b06ee 163 else{
howanglam3 3:759af41b06ee 164 out1 = 0;
howanglam3 3:759af41b06ee 165 out2 = 0;
howanglam3 3:759af41b06ee 166 }
howanglam3 3:759af41b06ee 167 if (y==1){ // pump air of pnumetic part
howanglam3 3:759af41b06ee 168 pin_B = 1;
howanglam3 3:759af41b06ee 169 }
howanglam3 3:759af41b06ee 170 else if(y==-1) { // release air of pnumetic part
howanglam3 3:759af41b06ee 171 pin_C = 1;
howanglam3 3:759af41b06ee 172 }
howanglam3 3:759af41b06ee 173 else{
howanglam3 3:759af41b06ee 174 pin_B = 0;
howanglam3 3:759af41b06ee 175 pin_C = 0;
howanglam3 3:759af41b06ee 176 }
howanglam3 2:399bcafe6f36 177 }
howanglam3 2:399bcafe6f36 178
bensonsinsin998 0:858c61a9c2de 179 Subscriber<Bool> subsqu("button_square", &messagesqu);
bensonsinsin998 0:858c61a9c2de 180 Subscriber<Bool> subcro("button_cross", &messagecro);
bensonsinsin998 0:858c61a9c2de 181 Subscriber<Bool> subcir("button_circle", &messagecir);
bensonsinsin998 0:858c61a9c2de 182 Subscriber<Bool> subtri("button_triangle", &messagetri);
howanglam3 2:399bcafe6f36 183 Subscriber<Twist> subkeypad("button_keypad", &messagekeypad);
bensonsinsin998 0:858c61a9c2de 184
bensonsinsin998 0:858c61a9c2de 185 int main() {
bensonsinsin998 0:858c61a9c2de 186 myled = 0;
bensonsinsin998 0:858c61a9c2de 187 servo = 0;
bensonsinsin998 0:858c61a9c2de 188
howanglam3 3:759af41b06ee 189 nh.getHardware()->setBaud(BAUD_RATE); //set baud rate
bensonsinsin998 0:858c61a9c2de 190 nh.initNode();
bensonsinsin998 0:858c61a9c2de 191
bensonsinsin998 0:858c61a9c2de 192 nh.subscribe(subsqu);
bensonsinsin998 0:858c61a9c2de 193 nh.subscribe(subcro);
bensonsinsin998 0:858c61a9c2de 194 nh.subscribe(subcir);
bensonsinsin998 0:858c61a9c2de 195 nh.subscribe(subtri);
howanglam3 3:759af41b06ee 196 nh.subscribe(subkeypad);
bensonsinsin998 0:858c61a9c2de 197 while(true) {
bensonsinsin998 0:858c61a9c2de 198 nh.spinOnce();
bensonsinsin998 0:858c61a9c2de 199 if(nh.connected())
bensonsinsin998 0:858c61a9c2de 200 myled = 1;
bensonsinsin998 0:858c61a9c2de 201 else
bensonsinsin998 0:858c61a9c2de 202 myled = 0;
bensonsinsin998 0:858c61a9c2de 203 }
bensonsinsin998 0:858c61a9c2de 204 }