Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@3:2441bcef5885, 2021-06-09 (annotated)

Committer:
ftppr
Date:
Wed Jun 09 10:36:41 2021 +0000
Revision:
3:2441bcef5885
Parent:
2:e570143bf35e
Child:
4:d5502eb8adcd
Only need one more Faulhaber;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ftppr 0:1bf7e0755f40 1 // ROS Connect Baud Rate
ftppr 0:1bf7e0755f40 2 #define BAUD_RATE 115200
ftppr 0:1bf7e0755f40 3
ftppr 0:1bf7e0755f40 4 #include "mbed.h"
ftppr 0:1bf7e0755f40 5 #include "Servo.h"
ftppr 0:1bf7e0755f40 6 #include <ros.h>
ftppr 0:1bf7e0755f40 7 #include <std_msgs/Int8.h>
ftppr 0:1bf7e0755f40 8 #include <std_msgs/String.h>
ftppr 0:1bf7e0755f40 9 #include <std_msgs/Int16MultiArray.h>
ftppr 2:e570143bf35e 10 #include "DC_Motor_Controller.h"
ftppr 2:e570143bf35e 11
ftppr 2:e570143bf35e 12 // Declare Functions
ftppr 0:1bf7e0755f40 13 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg);
ftppr 0:1bf7e0755f40 14 void msgCb_button(const std_msgs::Int8 &button_msg);
ftppr 2:e570143bf35e 15 void itoa(int num, char *str, int radix);
ftppr 0:1bf7e0755f40 16
ftppr 2:e570143bf35e 17 // Define ros variables
ftppr 2:e570143bf35e 18 ros::NodeHandle nh;
ftppr 2:e570143bf35e 19 // Subs
ftppr 0:1bf7e0755f40 20 ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick);
ftppr 0:1bf7e0755f40 21 ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button);
ftppr 2:e570143bf35e 22 // Pubs
ftppr 0:1bf7e0755f40 23 std_msgs::String str_msg;
ftppr 0:1bf7e0755f40 24 ros::Publisher responser("display", &str_msg);
ftppr 0:1bf7e0755f40 25
ftppr 2:e570143bf35e 26 // ============ Define IOs =============
ftppr 0:1bf7e0755f40 27
ftppr 2:e570143bf35e 28 // External Button
ftppr 0:1bf7e0755f40 29 DigitalIn button(USER_BUTTON);
ftppr 2:e570143bf35e 30 // LED
ftppr 2:e570143bf35e 31 DigitalOut myled(LED1);
ftppr 2:e570143bf35e 32 // Faulhaber pins
ftppr 2:e570143bf35e 33 DigitalOut up = A3; // pin 1 of faulhaber
ftppr 0:1bf7e0755f40 34 DigitalOut down = A5; // pin 2 of faulhaber
ftppr 0:1bf7e0755f40 35 DigitalOut back = A4; // pin 3 of faulhaber
ftppr 2:e570143bf35e 36 // Electromagnet
ftppr 2:e570143bf35e 37 DigitalOut lock = D14;
ftppr 2:e570143bf35e 38 // Firing pin
ftppr 2:e570143bf35e 39 DigitalIn firing1 = D3; // Reset Faulhaber
ftppr 2:e570143bf35e 40 DigitalIn firing2 = D15; // Load 1 arrow
ftppr 2:e570143bf35e 41 DigitalIn firing3 = D2; // Pick, small
ftppr 2:e570143bf35e 42 // Valves
ftppr 2:e570143bf35e 43 DigitalOut eject_1 = A1; // valve 1
ftppr 2:e570143bf35e 44 DigitalOut eject_2 = A2; // valve 2
ftppr 2:e570143bf35e 45 // DC motors
ftppr 3:2441bcef5885 46 DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; load 1 arrow
ftppr 3:2441bcef5885 47 DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; picker
ftppr 2:e570143bf35e 48
ftppr 2:e570143bf35e 49 // Global Control variable
ftppr 2:e570143bf35e 50 char msg[200] = {0}; // msg for responser
ftppr 2:e570143bf35e 51 int stage_pick = 0; // stage of picking up arrows from the rack
ftppr 2:e570143bf35e 52 int n_loaded = 0; // number of arrows.
ftppr 2:e570143bf35e 53 int faul_reset = 0;
ftppr 2:e570143bf35e 54 int load_manual = 1; // tune the loading 1 arrow manually
ftppr 2:e570143bf35e 55
ftppr 2:e570143bf35e 56 // button_msg: Int8
ftppr 2:e570143bf35e 57 void msgCb_button(const std_msgs::Int8 &button_msg)
ftppr 2:e570143bf35e 58 {
ftppr 2:e570143bf35e 59 memset(msg, 0, sizeof(msg));
ftppr 2:e570143bf35e 60 strcpy(msg, "Received ");
ftppr 2:e570143bf35e 61 itoa(button_msg.data, msg + 9, 10);
ftppr 2:e570143bf35e 62 int len = strlen(msg);
ftppr 2:e570143bf35e 63 msg[len++] = ';';
ftppr 2:e570143bf35e 64 msg[len++] = ' ';
ftppr 2:e570143bf35e 65
ftppr 2:e570143bf35e 66 switch (button_msg.data)
ftppr 2:e570143bf35e 67 {
ftppr 2:e570143bf35e 68 case 1:
ftppr 2:e570143bf35e 69 eject_1 = 1 - eject_1;
ftppr 2:e570143bf35e 70 eject_2 = 1 - eject_2;
ftppr 2:e570143bf35e 71 strcpy(msg + len, "x is pressed, eject_1 is ");
ftppr 2:e570143bf35e 72 msg[strlen(msg)] = '0' + int(eject_1);
ftppr 2:e570143bf35e 73 break;
ftppr 2:e570143bf35e 74 case -1:
ftppr 2:e570143bf35e 75 strcpy(msg + len, "x is released, eject_2 is ");
ftppr 2:e570143bf35e 76 msg[strlen(msg)] = '0' + int(eject_2);
ftppr 2:e570143bf35e 77 break;
ftppr 2:e570143bf35e 78 case 2:
ftppr 2:e570143bf35e 79 lock = 1 - lock;
ftppr 2:e570143bf35e 80 strcpy(msg + len, "o is pressed, lock is: ");
ftppr 2:e570143bf35e 81 msg[strlen(msg)] = '0' + int(lock);
ftppr 2:e570143bf35e 82
ftppr 2:e570143bf35e 83 break;
ftppr 2:e570143bf35e 84 case -2:
ftppr 2:e570143bf35e 85 strcpy(msg + len, "o is released");
ftppr 2:e570143bf35e 86 break;
ftppr 2:e570143bf35e 87 case 3:
ftppr 2:e570143bf35e 88 back = 1;
ftppr 2:e570143bf35e 89 strcpy(msg + len, "triangle is pressed, back is ");
ftppr 2:e570143bf35e 90 msg[strlen(msg)] = '0' + int(back);
ftppr 2:e570143bf35e 91 break;
ftppr 2:e570143bf35e 92 case -3:
ftppr 2:e570143bf35e 93 back = 0;
ftppr 2:e570143bf35e 94 strcpy(msg + len, "triangle is released, back is ");
ftppr 2:e570143bf35e 95 msg[strlen(msg)] = '0' + int(back);
ftppr 2:e570143bf35e 96 break;
ftppr 2:e570143bf35e 97 case 4:
ftppr 2:e570143bf35e 98 strcpy(msg + len, "square is pressed");
ftppr 2:e570143bf35e 99 up = 1; down = 1;
ftppr 2:e570143bf35e 100 break;
ftppr 2:e570143bf35e 101 case -4:
ftppr 2:e570143bf35e 102 strcpy(msg + len, "square is released");
ftppr 2:e570143bf35e 103 up = 0; down = 0;
ftppr 2:e570143bf35e 104 break;
ftppr 2:e570143bf35e 105 case 5:
ftppr 2:e570143bf35e 106 down = 1;
ftppr 2:e570143bf35e 107 up = 0;
ftppr 2:e570143bf35e 108 strcpy(msg + len, "down arrow is pressed, down is ");
ftppr 2:e570143bf35e 109 msg[strlen(msg)] = '0' + int(down);
ftppr 2:e570143bf35e 110 break;
ftppr 2:e570143bf35e 111 case -5:
ftppr 2:e570143bf35e 112 down = 0;
ftppr 2:e570143bf35e 113 strcpy(msg + len, "down arrow is released, down is ");
ftppr 2:e570143bf35e 114 msg[strlen(msg)] = '0' + int(down);
ftppr 2:e570143bf35e 115 break;
ftppr 2:e570143bf35e 116 case 6:
ftppr 2:e570143bf35e 117 load_manual = 0;
ftppr 2:e570143bf35e 118 strcpy(msg + len, "right arrow is pressed. ");
ftppr 2:e570143bf35e 119 msg[strlen(msg)] = '0' + n_loaded;
ftppr 2:e570143bf35e 120 dc_motor1.set_out(0,1);
ftppr 2:e570143bf35e 121 n_loaded++;
ftppr 3:2441bcef5885 122 while(firing2 == 0);
ftppr 3:2441bcef5885 123 // if(firing2 == 1)
ftppr 3:2441bcef5885 124 // {
ftppr 2:e570143bf35e 125 if(n_loaded <= 5)
ftppr 2:e570143bf35e 126 {
ftppr 3:2441bcef5885 127 dc_motor1.set_out(0,0.7);
ftppr 3:2441bcef5885 128 wait(1.2);
ftppr 3:2441bcef5885 129 dc_motor1.set_out(0,0);
ftppr 2:e570143bf35e 130 }
ftppr 2:e570143bf35e 131 else
ftppr 2:e570143bf35e 132 {
ftppr 2:e570143bf35e 133 strcpy(msg + len, "right arrow is pressed. ");
ftppr 2:e570143bf35e 134 strcpy(msg + strlen(msg), "n_loaded cleared!");
ftppr 2:e570143bf35e 135 n_loaded = 0;
ftppr 3:2441bcef5885 136 load_manual = 1;
ftppr 2:e570143bf35e 137 }
ftppr 3:2441bcef5885 138
ftppr 3:2441bcef5885 139 // }
ftppr 2:e570143bf35e 140 // load_manual = 1;
ftppr 2:e570143bf35e 141 break;
ftppr 2:e570143bf35e 142 case -6:
ftppr 2:e570143bf35e 143
ftppr 2:e570143bf35e 144 strcpy(msg + len, "right arrow is released. ");
ftppr 2:e570143bf35e 145 break;
ftppr 2:e570143bf35e 146 case 7:
ftppr 2:e570143bf35e 147 up = 1;
ftppr 2:e570143bf35e 148 down = 0;
ftppr 2:e570143bf35e 149 strcpy(msg + len, "up arrow is pressed, up is");
ftppr 2:e570143bf35e 150 msg[strlen(msg)] = '0' + int(up);
ftppr 2:e570143bf35e 151 break;
ftppr 2:e570143bf35e 152 case -7:
ftppr 2:e570143bf35e 153 up = 0;
ftppr 2:e570143bf35e 154 strcpy(msg + len, "up arrow is released, up is");
ftppr 2:e570143bf35e 155 msg[strlen(msg)] = '0' + int(up);
ftppr 2:e570143bf35e 156 break;
ftppr 2:e570143bf35e 157 case 8:
ftppr 2:e570143bf35e 158 strcpy(msg + len, "left arrow is pressed, left is ");
ftppr 2:e570143bf35e 159 msg[strlen(msg)] = '0' + stage_pick;
ftppr 2:e570143bf35e 160 if(stage_pick == 0)
ftppr 2:e570143bf35e 161 {
ftppr 2:e570143bf35e 162 dc_motor2.set_out(0, 0.5);
ftppr 2:e570143bf35e 163 if(firing3 == 1)
ftppr 2:e570143bf35e 164 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 165 stage_pick++;
ftppr 2:e570143bf35e 166 eject_1 = 1; eject_2 = 0;
ftppr 2:e570143bf35e 167 }
ftppr 2:e570143bf35e 168 else if(stage_pick == 1)
ftppr 2:e570143bf35e 169 {
ftppr 2:e570143bf35e 170 dc_motor2.set_out(0.5,0);
ftppr 2:e570143bf35e 171 wait(0.3);
ftppr 2:e570143bf35e 172 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 173 stage_pick++;
ftppr 2:e570143bf35e 174 }
ftppr 2:e570143bf35e 175 else if(stage_pick == 2)
ftppr 2:e570143bf35e 176 {
ftppr 2:e570143bf35e 177 dc_motor2.set_out(0.5,0);
ftppr 2:e570143bf35e 178 wait(0.3);
ftppr 2:e570143bf35e 179 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 180 stage_pick = 0;
ftppr 2:e570143bf35e 181 }
ftppr 2:e570143bf35e 182 break;
ftppr 2:e570143bf35e 183 case -8:
ftppr 2:e570143bf35e 184 strcpy(msg + len, "left arrow is released, left is ");
ftppr 2:e570143bf35e 185 break;
ftppr 2:e570143bf35e 186 case 9:
ftppr 2:e570143bf35e 187 strcpy(msg + len, "L1 is pressed");
ftppr 2:e570143bf35e 188 break;
ftppr 2:e570143bf35e 189 case -9:
ftppr 2:e570143bf35e 190 strcpy(msg + len, "L1 is released");
ftppr 2:e570143bf35e 191 break;
ftppr 2:e570143bf35e 192 case 10:
ftppr 2:e570143bf35e 193 dc_motor1.set_out(0,0);
ftppr 2:e570143bf35e 194 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 195 strcpy(msg + len, "R1 is pressed");
ftppr 2:e570143bf35e 196 break;
ftppr 2:e570143bf35e 197 case -10:
ftppr 2:e570143bf35e 198 strcpy(msg + len, "R1 is released");
ftppr 2:e570143bf35e 199 break;
ftppr 2:e570143bf35e 200 case 11:
ftppr 2:e570143bf35e 201 strcpy(msg + len, "share is pressed");
ftppr 2:e570143bf35e 202 break;
ftppr 2:e570143bf35e 203 case -11:
ftppr 2:e570143bf35e 204 strcpy(msg + len, "share is released");
ftppr 2:e570143bf35e 205 break;
ftppr 2:e570143bf35e 206 case 12:
ftppr 2:e570143bf35e 207 strcpy(msg + len, "option is pressed");
ftppr 2:e570143bf35e 208 break;
ftppr 2:e570143bf35e 209 case -12:
ftppr 2:e570143bf35e 210 strcpy(msg + len, "option is released");
ftppr 2:e570143bf35e 211 break;
ftppr 2:e570143bf35e 212 case 13:
ftppr 2:e570143bf35e 213 up = 1; down = 1; back = 1;
ftppr 2:e570143bf35e 214 strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
ftppr 2:e570143bf35e 215 break;
ftppr 2:e570143bf35e 216 case -13:
ftppr 2:e570143bf35e 217 up = 0; down = 0; back= 0;
ftppr 2:e570143bf35e 218 strcpy(msg + len, "playstation button is released");
ftppr 2:e570143bf35e 219 break;
ftppr 2:e570143bf35e 220 default:
ftppr 2:e570143bf35e 221 len = strlen(msg);
ftppr 2:e570143bf35e 222 strcpy(msg + len, "Unrecognized pattern");
ftppr 2:e570143bf35e 223 break;
ftppr 2:e570143bf35e 224 }
ftppr 2:e570143bf35e 225
ftppr 2:e570143bf35e 226 responser.publish(&str_msg);
ftppr 2:e570143bf35e 227 }
ftppr 2:e570143bf35e 228
ftppr 2:e570143bf35e 229
ftppr 2:e570143bf35e 230 // --------------- main -----------------
ftppr 2:e570143bf35e 231 int main()
ftppr 2:e570143bf35e 232 {
ftppr 2:e570143bf35e 233
ftppr 2:e570143bf35e 234 // -> Set Baud Rate
ftppr 2:e570143bf35e 235 nh.getHardware()->setBaud(BAUD_RATE);
ftppr 2:e570143bf35e 236 nh.initNode();
ftppr 2:e570143bf35e 237 // Advertise publishers
ftppr 2:e570143bf35e 238 nh.advertise(responser);
ftppr 2:e570143bf35e 239 // Initiate display
ftppr 2:e570143bf35e 240 str_msg.data = msg;
ftppr 2:e570143bf35e 241 // responser.publish(&str_msg);
ftppr 2:e570143bf35e 242 // Initiate subscribers
ftppr 2:e570143bf35e 243 nh.subscribe(sub_button);
ftppr 2:e570143bf35e 244 nh.subscribe(sub_joystick);
ftppr 2:e570143bf35e 245 // Initiate IOs, keep everything at rest
ftppr 2:e570143bf35e 246 myled = 0;
ftppr 2:e570143bf35e 247 back = 0;up = 0; down = 0; lock = 0;
ftppr 2:e570143bf35e 248 eject_1 = 0; eject_2 = 1;
ftppr 2:e570143bf35e 249
ftppr 2:e570143bf35e 250 // Initiate firing pins
ftppr 2:e570143bf35e 251 firing1.mode(PullUp); // default 1
ftppr 3:2441bcef5885 252 firing2.mode(PullUp); // default 1
ftppr 2:e570143bf35e 253 firing3.mode(PullUp); // default 1
ftppr 2:e570143bf35e 254
ftppr 2:e570143bf35e 255 // Reset dc motor
ftppr 2:e570143bf35e 256 dc_motor1.reset();
ftppr 2:e570143bf35e 257 dc_motor1.set_pid(0.008, 0, 0, 0.0);
ftppr 2:e570143bf35e 258 dc_motor2.reset();
ftppr 2:e570143bf35e 259 dc_motor2.set_pid(0.008, 0, 0, 0.0);
ftppr 3:2441bcef5885 260 // dc_motor2.set_out(0,0.5); //picker
ftppr 2:e570143bf35e 261 // wait(0.3);
ftppr 2:e570143bf35e 262 // dc_motor2.move_angle(100);
ftppr 3:2441bcef5885 263 // dc_motor2.set_out(0,1);
ftppr 2:e570143bf35e 264 // dc_motor1.move_angle(100);
ftppr 2:e570143bf35e 265 // dc_motor1.set_out(0.5,0); // -> testing only
ftppr 2:e570143bf35e 266
ftppr 2:e570143bf35e 267 // Main programming
ftppr 2:e570143bf35e 268 int start = 0; // control variable for resetting shooter
ftppr 2:e570143bf35e 269 int tmp = 0;
ftppr 2:e570143bf35e 270
ftppr 2:e570143bf35e 271 while(true) {
ftppr 2:e570143bf35e 272 nh.spinOnce();
ftppr 2:e570143bf35e 273
ftppr 2:e570143bf35e 274 if(nh.connected())
ftppr 2:e570143bf35e 275 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
ftppr 2:e570143bf35e 276 else
ftppr 2:e570143bf35e 277 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
ftppr 2:e570143bf35e 278 if(button == 1)
ftppr 2:e570143bf35e 279 {
ftppr 2:e570143bf35e 280 myled = 0;
ftppr 3:2441bcef5885 281 if(!faul_reset && firing1 == 0)
ftppr 2:e570143bf35e 282 {
ftppr 2:e570143bf35e 283 down = 0; up = 1; back = 1; // 101
ftppr 2:e570143bf35e 284 start = 1;
ftppr 2:e570143bf35e 285 faul_reset = 1;
ftppr 2:e570143bf35e 286 }
ftppr 2:e570143bf35e 287 else
ftppr 2:e570143bf35e 288 {
ftppr 3:2441bcef5885 289 dc_motor1.set_out(0,1);
ftppr 3:2441bcef5885 290 load_manual = 1;
ftppr 2:e570143bf35e 291 }
ftppr 2:e570143bf35e 292 while(button == 1);
ftppr 2:e570143bf35e 293 wait(0.2);
ftppr 2:e570143bf35e 294 memset(msg, 0, sizeof(msg));
ftppr 2:e570143bf35e 295 strcpy(msg, "user button is pressed, faul_reset is ");
ftppr 3:2441bcef5885 296 msg[strlen(msg)] = '0' + faul_reset;
ftppr 3:2441bcef5885 297 strcpy(msg + strlen(msg), ", firing2 is ");
ftppr 3:2441bcef5885 298 msg[strlen(msg)] = '0' + firing2.read();
ftppr 3:2441bcef5885 299 strcpy(msg + strlen(msg), ", load_manual is ");
ftppr 3:2441bcef5885 300 msg[strlen(msg)] = '0' + load_manual;
ftppr 2:e570143bf35e 301 responser.publish(&str_msg);
ftppr 2:e570143bf35e 302 }
ftppr 2:e570143bf35e 303 if(load_manual && firing2 == 1)
ftppr 2:e570143bf35e 304 {
ftppr 3:2441bcef5885 305 wait(0.17);
ftppr 3:2441bcef5885 306 dc_motor1.set_out(0,0);
ftppr 3:2441bcef5885 307 memset(msg, 0, sizeof(msg));
ftppr 3:2441bcef5885 308 strcpy(msg, "firing2 is touched: ");
ftppr 3:2441bcef5885 309 msg[strlen(msg)] = '0' + firing2.read();
ftppr 2:e570143bf35e 310 }
ftppr 3:2441bcef5885 311 if(firing1.read() == 1 && start == 1)
ftppr 2:e570143bf35e 312 {
ftppr 2:e570143bf35e 313 up = 0; down = 1; back = 1; // 110
ftppr 2:e570143bf35e 314 wait(0.3);
ftppr 2:e570143bf35e 315 memset(msg, 0, sizeof(msg));
ftppr 2:e570143bf35e 316 strcpy(msg, "firing1 is touched: ");
ftppr 2:e570143bf35e 317 msg[strlen(msg)] = '0' + firing1.read();
ftppr 2:e570143bf35e 318 responser.publish(&str_msg);
ftppr 3:2441bcef5885 319 down = 0; back = 0; up = 0;
ftppr 3:2441bcef5885 320 start = 0;
ftppr 2:e570143bf35e 321
ftppr 2:e570143bf35e 322 }
ftppr 2:e570143bf35e 323 }
ftppr 2:e570143bf35e 324 }
ftppr 0:1bf7e0755f40 325
ftppr 0:1bf7e0755f40 326
ftppr 0:1bf7e0755f40 327 void itoa(int num, char *str, int radix)
ftppr 0:1bf7e0755f40 328 { /*索引表*/
ftppr 0:1bf7e0755f40 329 char index[] = "0123456789ABCDEF";
ftppr 0:1bf7e0755f40 330 unsigned unum; /*中间变量*/
ftppr 0:1bf7e0755f40 331 int i = 0, j, k;
ftppr 0:1bf7e0755f40 332 /*确定unum的值*/
ftppr 0:1bf7e0755f40 333 if (radix == 10 && num < 0) /*十进制负数*/
ftppr 0:1bf7e0755f40 334 {
ftppr 0:1bf7e0755f40 335 unum = (unsigned)-num;
ftppr 0:1bf7e0755f40 336 str[i++] = '-';
ftppr 0:1bf7e0755f40 337 }
ftppr 0:1bf7e0755f40 338 else
ftppr 0:1bf7e0755f40 339 unum = (unsigned)num; /*其他情况*/
ftppr 0:1bf7e0755f40 340 /*转换*/
ftppr 0:1bf7e0755f40 341 do
ftppr 0:1bf7e0755f40 342 {
ftppr 0:1bf7e0755f40 343 str[i++] = index[unum % (unsigned)radix];
ftppr 0:1bf7e0755f40 344 unum /= radix;
ftppr 0:1bf7e0755f40 345 } while (unum);
ftppr 0:1bf7e0755f40 346 str[i] = '\0';
ftppr 0:1bf7e0755f40 347 /*逆序*/
ftppr 0:1bf7e0755f40 348 if (str[0] == '-')
ftppr 0:1bf7e0755f40 349 k = 1; /*十进制负数*/
ftppr 0:1bf7e0755f40 350 else
ftppr 0:1bf7e0755f40 351 k = 0;
ftppr 0:1bf7e0755f40 352
ftppr 0:1bf7e0755f40 353 for (j = k; j <= (i - 1) / 2; j++)
ftppr 0:1bf7e0755f40 354 {
ftppr 0:1bf7e0755f40 355 char temp;
ftppr 0:1bf7e0755f40 356 temp = str[j];
ftppr 0:1bf7e0755f40 357 str[j] = str[i - 1 + k - j];
ftppr 0:1bf7e0755f40 358 str[i - 1 + k - j] = temp;
ftppr 0:1bf7e0755f40 359 }
ftppr 0:1bf7e0755f40 360 }
ftppr 0:1bf7e0755f40 361
ftppr 0:1bf7e0755f40 362 /*
ftppr 0:1bf7e0755f40 363 * joystick_msg: (Left/Right, x/y, value)
ftppr 0:1bf7e0755f40 364 */
ftppr 0:1bf7e0755f40 365
ftppr 0:1bf7e0755f40 366 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg)
ftppr 0:1bf7e0755f40 367 {
ftppr 0:1bf7e0755f40 368 // myled = !myled; // blink the led
ftppr 0:1bf7e0755f40 369 memset(msg, 0, sizeof(msg));
ftppr 0:1bf7e0755f40 370 // eg. Received (0,0,-100)
ftppr 0:1bf7e0755f40 371 strcpy(msg, "Received (");
ftppr 0:1bf7e0755f40 372 int len = strlen(msg);
ftppr 0:1bf7e0755f40 373 msg[len++] = '0' + joystick_msg.data[0];
ftppr 0:1bf7e0755f40 374 msg[len++] = ',';
ftppr 0:1bf7e0755f40 375 itoa(joystick_msg.data[1], msg + len, 10);
ftppr 0:1bf7e0755f40 376 len = strlen(msg);
ftppr 0:1bf7e0755f40 377 msg[len++] = ',';
ftppr 0:1bf7e0755f40 378 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 379 len = strlen(msg);
ftppr 0:1bf7e0755f40 380 msg[len++] = ')';
ftppr 0:1bf7e0755f40 381 msg[len++] = ';';
ftppr 0:1bf7e0755f40 382 msg[len++] = ' ';
ftppr 0:1bf7e0755f40 383
ftppr 0:1bf7e0755f40 384 // Elaboration of the message
ftppr 0:1bf7e0755f40 385 // -----------L3------------
ftppr 0:1bf7e0755f40 386 // x
ftppr 0:1bf7e0755f40 387 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0)
ftppr 0:1bf7e0755f40 388 {
ftppr 0:1bf7e0755f40 389 strcpy(msg + len, "L3: direction is x(horizontal) and the value is ");
ftppr 0:1bf7e0755f40 390 len = strlen(msg);
ftppr 0:1bf7e0755f40 391 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 392 }
ftppr 0:1bf7e0755f40 393 // y
ftppr 0:1bf7e0755f40 394 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1)
ftppr 0:1bf7e0755f40 395 {
ftppr 0:1bf7e0755f40 396 strcpy(msg + len, "L3: direction is y(vertical) and the value is ");
ftppr 0:1bf7e0755f40 397 len = strlen(msg);
ftppr 0:1bf7e0755f40 398 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 399 }
ftppr 0:1bf7e0755f40 400
ftppr 0:1bf7e0755f40 401 // -----------R3------------
ftppr 0:1bf7e0755f40 402 // x
ftppr 0:1bf7e0755f40 403 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0)
ftppr 0:1bf7e0755f40 404 {
ftppr 0:1bf7e0755f40 405 strcpy(msg + len, "R3: direction is x(horizontal) and the value is ");
ftppr 0:1bf7e0755f40 406 len = strlen(msg);
ftppr 0:1bf7e0755f40 407 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 408 }
ftppr 0:1bf7e0755f40 409 // y
ftppr 0:1bf7e0755f40 410 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1)
ftppr 0:1bf7e0755f40 411 {
ftppr 0:1bf7e0755f40 412 strcpy(msg + len, "R3: direction is y(vertical) and the value is ");
ftppr 0:1bf7e0755f40 413 len = strlen(msg);
ftppr 0:1bf7e0755f40 414 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 415 }
ftppr 0:1bf7e0755f40 416
ftppr 0:1bf7e0755f40 417 // -----------L2------------
ftppr 0:1bf7e0755f40 418 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1)
ftppr 0:1bf7e0755f40 419 {
ftppr 0:1bf7e0755f40 420 strcpy(msg + len, "L2: the value is ");
ftppr 0:1bf7e0755f40 421 len = strlen(msg);
ftppr 0:1bf7e0755f40 422 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 423 }
ftppr 0:1bf7e0755f40 424 // -----------R2------------
ftppr 0:1bf7e0755f40 425 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
ftppr 0:1bf7e0755f40 426 {
ftppr 0:1bf7e0755f40 427 strcpy(msg + len, "L3: the value is ");
ftppr 0:1bf7e0755f40 428 len = strlen(msg);
ftppr 0:1bf7e0755f40 429 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 2:e570143bf35e 430
ftppr 0:1bf7e0755f40 431 }
ftppr 0:1bf7e0755f40 432
ftppr 0:1bf7e0755f40 433 responser.publish(&str_msg);
ftppr 0:1bf7e0755f40 434 }