Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@4:d5502eb8adcd, 2021-06-11 (annotated)

Committer:
ftppr
Date:
Fri Jun 11 02:19:26 2021 +0000
Revision:
4:d5502eb8adcd
Parent:
3:2441bcef5885
Child:
5:03af248f3f7c
you see see what different la;

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 4:d5502eb8adcd 99 up = 1; down = 1; lock = 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 4:d5502eb8adcd 123
ftppr 4:d5502eb8adcd 124 if(n_loaded <= 5)
ftppr 4:d5502eb8adcd 125 {
ftppr 4:d5502eb8adcd 126 dc_motor1.set_out(0,0.7);
ftppr 4:d5502eb8adcd 127 wait(1.4);
ftppr 4:d5502eb8adcd 128 dc_motor1.set_out(0,0);
ftppr 4:d5502eb8adcd 129 }
ftppr 4:d5502eb8adcd 130 else
ftppr 4:d5502eb8adcd 131 {
ftppr 4:d5502eb8adcd 132 load_manual = 1;
ftppr 4:d5502eb8adcd 133 strcpy(msg + len, "right arrow is pressed. ");
ftppr 4:d5502eb8adcd 134 strcpy(msg + strlen(msg), "n_loaded cleared!");
ftppr 4:d5502eb8adcd 135 n_loaded = 0;
ftppr 3:2441bcef5885 136
ftppr 4:d5502eb8adcd 137 }
ftppr 4:d5502eb8adcd 138
ftppr 2:e570143bf35e 139 break;
ftppr 2:e570143bf35e 140 case -6:
ftppr 2:e570143bf35e 141
ftppr 2:e570143bf35e 142 strcpy(msg + len, "right arrow is released. ");
ftppr 2:e570143bf35e 143 break;
ftppr 2:e570143bf35e 144 case 7:
ftppr 2:e570143bf35e 145 up = 1;
ftppr 2:e570143bf35e 146 down = 0;
ftppr 2:e570143bf35e 147 strcpy(msg + len, "up arrow is pressed, up is");
ftppr 2:e570143bf35e 148 msg[strlen(msg)] = '0' + int(up);
ftppr 2:e570143bf35e 149 break;
ftppr 2:e570143bf35e 150 case -7:
ftppr 2:e570143bf35e 151 up = 0;
ftppr 2:e570143bf35e 152 strcpy(msg + len, "up arrow is released, up is");
ftppr 2:e570143bf35e 153 msg[strlen(msg)] = '0' + int(up);
ftppr 2:e570143bf35e 154 break;
ftppr 2:e570143bf35e 155 case 8:
ftppr 2:e570143bf35e 156 strcpy(msg + len, "left arrow is pressed, left is ");
ftppr 2:e570143bf35e 157 msg[strlen(msg)] = '0' + stage_pick;
ftppr 2:e570143bf35e 158 if(stage_pick == 0)
ftppr 2:e570143bf35e 159 {
ftppr 2:e570143bf35e 160 dc_motor2.set_out(0, 0.5);
ftppr 2:e570143bf35e 161 if(firing3 == 1)
ftppr 2:e570143bf35e 162 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 163 stage_pick++;
ftppr 2:e570143bf35e 164 eject_1 = 1; eject_2 = 0;
ftppr 2:e570143bf35e 165 }
ftppr 2:e570143bf35e 166 else if(stage_pick == 1)
ftppr 2:e570143bf35e 167 {
ftppr 2:e570143bf35e 168 dc_motor2.set_out(0.5,0);
ftppr 2:e570143bf35e 169 wait(0.3);
ftppr 2:e570143bf35e 170 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 171 stage_pick++;
ftppr 2:e570143bf35e 172 }
ftppr 2:e570143bf35e 173 else if(stage_pick == 2)
ftppr 2:e570143bf35e 174 {
ftppr 2:e570143bf35e 175 dc_motor2.set_out(0.5,0);
ftppr 2:e570143bf35e 176 wait(0.3);
ftppr 2:e570143bf35e 177 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 178 stage_pick = 0;
ftppr 2:e570143bf35e 179 }
ftppr 2:e570143bf35e 180 break;
ftppr 2:e570143bf35e 181 case -8:
ftppr 2:e570143bf35e 182 strcpy(msg + len, "left arrow is released, left is ");
ftppr 2:e570143bf35e 183 break;
ftppr 2:e570143bf35e 184 case 9:
ftppr 2:e570143bf35e 185 strcpy(msg + len, "L1 is pressed");
ftppr 2:e570143bf35e 186 break;
ftppr 2:e570143bf35e 187 case -9:
ftppr 2:e570143bf35e 188 strcpy(msg + len, "L1 is released");
ftppr 2:e570143bf35e 189 break;
ftppr 2:e570143bf35e 190 case 10:
ftppr 2:e570143bf35e 191 dc_motor1.set_out(0,0);
ftppr 2:e570143bf35e 192 dc_motor2.set_out(0,0);
ftppr 2:e570143bf35e 193 strcpy(msg + len, "R1 is pressed");
ftppr 2:e570143bf35e 194 break;
ftppr 2:e570143bf35e 195 case -10:
ftppr 2:e570143bf35e 196 strcpy(msg + len, "R1 is released");
ftppr 2:e570143bf35e 197 break;
ftppr 2:e570143bf35e 198 case 11:
ftppr 2:e570143bf35e 199 strcpy(msg + len, "share is pressed");
ftppr 2:e570143bf35e 200 break;
ftppr 2:e570143bf35e 201 case -11:
ftppr 2:e570143bf35e 202 strcpy(msg + len, "share is released");
ftppr 2:e570143bf35e 203 break;
ftppr 2:e570143bf35e 204 case 12:
ftppr 2:e570143bf35e 205 strcpy(msg + len, "option is pressed");
ftppr 2:e570143bf35e 206 break;
ftppr 2:e570143bf35e 207 case -12:
ftppr 2:e570143bf35e 208 strcpy(msg + len, "option is released");
ftppr 2:e570143bf35e 209 break;
ftppr 2:e570143bf35e 210 case 13:
ftppr 2:e570143bf35e 211 up = 1; down = 1; back = 1;
ftppr 2:e570143bf35e 212 strcpy(msg + len, "playstation button is pressed, cur pos should be saved");
ftppr 2:e570143bf35e 213 break;
ftppr 2:e570143bf35e 214 case -13:
ftppr 2:e570143bf35e 215 up = 0; down = 0; back= 0;
ftppr 2:e570143bf35e 216 strcpy(msg + len, "playstation button is released");
ftppr 2:e570143bf35e 217 break;
ftppr 2:e570143bf35e 218 default:
ftppr 2:e570143bf35e 219 len = strlen(msg);
ftppr 2:e570143bf35e 220 strcpy(msg + len, "Unrecognized pattern");
ftppr 2:e570143bf35e 221 break;
ftppr 2:e570143bf35e 222 }
ftppr 2:e570143bf35e 223
ftppr 2:e570143bf35e 224 responser.publish(&str_msg);
ftppr 2:e570143bf35e 225 }
ftppr 2:e570143bf35e 226
ftppr 2:e570143bf35e 227
ftppr 2:e570143bf35e 228 // --------------- main -----------------
ftppr 2:e570143bf35e 229 int main()
ftppr 2:e570143bf35e 230 {
ftppr 2:e570143bf35e 231
ftppr 2:e570143bf35e 232 // -> Set Baud Rate
ftppr 2:e570143bf35e 233 nh.getHardware()->setBaud(BAUD_RATE);
ftppr 2:e570143bf35e 234 nh.initNode();
ftppr 2:e570143bf35e 235 // Advertise publishers
ftppr 2:e570143bf35e 236 nh.advertise(responser);
ftppr 2:e570143bf35e 237 // Initiate display
ftppr 2:e570143bf35e 238 str_msg.data = msg;
ftppr 2:e570143bf35e 239 // responser.publish(&str_msg);
ftppr 2:e570143bf35e 240 // Initiate subscribers
ftppr 2:e570143bf35e 241 nh.subscribe(sub_button);
ftppr 2:e570143bf35e 242 nh.subscribe(sub_joystick);
ftppr 2:e570143bf35e 243 // Initiate IOs, keep everything at rest
ftppr 2:e570143bf35e 244 myled = 0;
ftppr 2:e570143bf35e 245 back = 0;up = 0; down = 0; lock = 0;
ftppr 2:e570143bf35e 246 eject_1 = 0; eject_2 = 1;
ftppr 2:e570143bf35e 247
ftppr 2:e570143bf35e 248 // Initiate firing pins
ftppr 2:e570143bf35e 249 firing1.mode(PullUp); // default 1
ftppr 3:2441bcef5885 250 firing2.mode(PullUp); // default 1
ftppr 2:e570143bf35e 251 firing3.mode(PullUp); // default 1
ftppr 2:e570143bf35e 252
ftppr 2:e570143bf35e 253 // Reset dc motor
ftppr 2:e570143bf35e 254 dc_motor1.reset();
ftppr 2:e570143bf35e 255 dc_motor1.set_pid(0.008, 0, 0, 0.0);
ftppr 2:e570143bf35e 256 dc_motor2.reset();
ftppr 2:e570143bf35e 257 dc_motor2.set_pid(0.008, 0, 0, 0.0);
ftppr 4:d5502eb8adcd 258 up = 1; back = 1; down = 0;
ftppr 2:e570143bf35e 259 // Main programming
ftppr 2:e570143bf35e 260 int start = 0; // control variable for resetting shooter
ftppr 2:e570143bf35e 261 int tmp = 0;
ftppr 2:e570143bf35e 262
ftppr 2:e570143bf35e 263 while(true) {
ftppr 2:e570143bf35e 264 nh.spinOnce();
ftppr 2:e570143bf35e 265
ftppr 2:e570143bf35e 266 if(nh.connected())
ftppr 2:e570143bf35e 267 myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson
ftppr 2:e570143bf35e 268 else
ftppr 2:e570143bf35e 269 myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson
ftppr 2:e570143bf35e 270 if(button == 1)
ftppr 2:e570143bf35e 271 {
ftppr 2:e570143bf35e 272 myled = 0;
ftppr 3:2441bcef5885 273 if(!faul_reset && firing1 == 0)
ftppr 4:d5502eb8adcd 274 // if(1)
ftppr 2:e570143bf35e 275 {
ftppr 2:e570143bf35e 276 down = 0; up = 1; back = 1; // 101
ftppr 2:e570143bf35e 277 start = 1;
ftppr 4:d5502eb8adcd 278 memset(msg, 0, sizeof(msg));
ftppr 4:d5502eb8adcd 279 strcpy(msg, "Start resetting Faulhaber: firing1 is ");
ftppr 4:d5502eb8adcd 280 msg[strlen(msg)] = '0' + firing1.read();
ftppr 4:d5502eb8adcd 281 responser.publish(&str_msg);
ftppr 4:d5502eb8adcd 282 // faul_reset = 1;
ftppr 2:e570143bf35e 283 }
ftppr 2:e570143bf35e 284 else
ftppr 2:e570143bf35e 285 {
ftppr 3:2441bcef5885 286 dc_motor1.set_out(0,1);
ftppr 3:2441bcef5885 287 load_manual = 1;
ftppr 2:e570143bf35e 288 }
ftppr 2:e570143bf35e 289 while(button == 1);
ftppr 2:e570143bf35e 290 wait(0.2);
ftppr 2:e570143bf35e 291 memset(msg, 0, sizeof(msg));
ftppr 2:e570143bf35e 292 strcpy(msg, "user button is pressed, faul_reset is ");
ftppr 3:2441bcef5885 293 msg[strlen(msg)] = '0' + faul_reset;
ftppr 3:2441bcef5885 294 strcpy(msg + strlen(msg), ", firing2 is ");
ftppr 3:2441bcef5885 295 msg[strlen(msg)] = '0' + firing2.read();
ftppr 3:2441bcef5885 296 strcpy(msg + strlen(msg), ", load_manual is ");
ftppr 3:2441bcef5885 297 msg[strlen(msg)] = '0' + load_manual;
ftppr 2:e570143bf35e 298 responser.publish(&str_msg);
ftppr 2:e570143bf35e 299 }
ftppr 2:e570143bf35e 300 if(load_manual && firing2 == 1)
ftppr 2:e570143bf35e 301 {
ftppr 3:2441bcef5885 302 wait(0.17);
ftppr 3:2441bcef5885 303 dc_motor1.set_out(0,0);
ftppr 3:2441bcef5885 304 memset(msg, 0, sizeof(msg));
ftppr 3:2441bcef5885 305 strcpy(msg, "firing2 is touched: ");
ftppr 3:2441bcef5885 306 msg[strlen(msg)] = '0' + firing2.read();
ftppr 2:e570143bf35e 307 }
ftppr 3:2441bcef5885 308 if(firing1.read() == 1 && start == 1)
ftppr 2:e570143bf35e 309 {
ftppr 2:e570143bf35e 310 up = 0; down = 1; back = 1; // 110
ftppr 2:e570143bf35e 311 wait(0.3);
ftppr 2:e570143bf35e 312 memset(msg, 0, sizeof(msg));
ftppr 2:e570143bf35e 313 strcpy(msg, "firing1 is touched: ");
ftppr 2:e570143bf35e 314 msg[strlen(msg)] = '0' + firing1.read();
ftppr 2:e570143bf35e 315 responser.publish(&str_msg);
ftppr 3:2441bcef5885 316 down = 0; back = 0; up = 0;
ftppr 3:2441bcef5885 317 start = 0;
ftppr 2:e570143bf35e 318
ftppr 2:e570143bf35e 319 }
ftppr 2:e570143bf35e 320 }
ftppr 2:e570143bf35e 321 }
ftppr 0:1bf7e0755f40 322
ftppr 0:1bf7e0755f40 323
ftppr 0:1bf7e0755f40 324 void itoa(int num, char *str, int radix)
ftppr 0:1bf7e0755f40 325 { /*索引表*/
ftppr 0:1bf7e0755f40 326 char index[] = "0123456789ABCDEF";
ftppr 0:1bf7e0755f40 327 unsigned unum; /*中间变量*/
ftppr 0:1bf7e0755f40 328 int i = 0, j, k;
ftppr 0:1bf7e0755f40 329 /*确定unum的值*/
ftppr 0:1bf7e0755f40 330 if (radix == 10 && num < 0) /*十进制负数*/
ftppr 0:1bf7e0755f40 331 {
ftppr 0:1bf7e0755f40 332 unum = (unsigned)-num;
ftppr 0:1bf7e0755f40 333 str[i++] = '-';
ftppr 0:1bf7e0755f40 334 }
ftppr 0:1bf7e0755f40 335 else
ftppr 0:1bf7e0755f40 336 unum = (unsigned)num; /*其他情况*/
ftppr 0:1bf7e0755f40 337 /*转换*/
ftppr 0:1bf7e0755f40 338 do
ftppr 0:1bf7e0755f40 339 {
ftppr 0:1bf7e0755f40 340 str[i++] = index[unum % (unsigned)radix];
ftppr 0:1bf7e0755f40 341 unum /= radix;
ftppr 0:1bf7e0755f40 342 } while (unum);
ftppr 0:1bf7e0755f40 343 str[i] = '\0';
ftppr 0:1bf7e0755f40 344 /*逆序*/
ftppr 0:1bf7e0755f40 345 if (str[0] == '-')
ftppr 0:1bf7e0755f40 346 k = 1; /*十进制负数*/
ftppr 0:1bf7e0755f40 347 else
ftppr 0:1bf7e0755f40 348 k = 0;
ftppr 0:1bf7e0755f40 349
ftppr 0:1bf7e0755f40 350 for (j = k; j <= (i - 1) / 2; j++)
ftppr 0:1bf7e0755f40 351 {
ftppr 0:1bf7e0755f40 352 char temp;
ftppr 0:1bf7e0755f40 353 temp = str[j];
ftppr 0:1bf7e0755f40 354 str[j] = str[i - 1 + k - j];
ftppr 0:1bf7e0755f40 355 str[i - 1 + k - j] = temp;
ftppr 0:1bf7e0755f40 356 }
ftppr 0:1bf7e0755f40 357 }
ftppr 0:1bf7e0755f40 358
ftppr 0:1bf7e0755f40 359 /*
ftppr 0:1bf7e0755f40 360 * joystick_msg: (Left/Right, x/y, value)
ftppr 0:1bf7e0755f40 361 */
ftppr 0:1bf7e0755f40 362
ftppr 0:1bf7e0755f40 363 void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg)
ftppr 0:1bf7e0755f40 364 {
ftppr 0:1bf7e0755f40 365 // myled = !myled; // blink the led
ftppr 0:1bf7e0755f40 366 memset(msg, 0, sizeof(msg));
ftppr 0:1bf7e0755f40 367 // eg. Received (0,0,-100)
ftppr 0:1bf7e0755f40 368 strcpy(msg, "Received (");
ftppr 0:1bf7e0755f40 369 int len = strlen(msg);
ftppr 0:1bf7e0755f40 370 msg[len++] = '0' + joystick_msg.data[0];
ftppr 0:1bf7e0755f40 371 msg[len++] = ',';
ftppr 0:1bf7e0755f40 372 itoa(joystick_msg.data[1], msg + len, 10);
ftppr 0:1bf7e0755f40 373 len = strlen(msg);
ftppr 0:1bf7e0755f40 374 msg[len++] = ',';
ftppr 0:1bf7e0755f40 375 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 376 len = strlen(msg);
ftppr 0:1bf7e0755f40 377 msg[len++] = ')';
ftppr 0:1bf7e0755f40 378 msg[len++] = ';';
ftppr 0:1bf7e0755f40 379 msg[len++] = ' ';
ftppr 0:1bf7e0755f40 380
ftppr 0:1bf7e0755f40 381 // Elaboration of the message
ftppr 0:1bf7e0755f40 382 // -----------L3------------
ftppr 0:1bf7e0755f40 383 // x
ftppr 0:1bf7e0755f40 384 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0)
ftppr 0:1bf7e0755f40 385 {
ftppr 0:1bf7e0755f40 386 strcpy(msg + len, "L3: direction is x(horizontal) and the value is ");
ftppr 0:1bf7e0755f40 387 len = strlen(msg);
ftppr 0:1bf7e0755f40 388 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 389 }
ftppr 0:1bf7e0755f40 390 // y
ftppr 0:1bf7e0755f40 391 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1)
ftppr 0:1bf7e0755f40 392 {
ftppr 0:1bf7e0755f40 393 strcpy(msg + len, "L3: direction is y(vertical) and the value is ");
ftppr 0:1bf7e0755f40 394 len = strlen(msg);
ftppr 0:1bf7e0755f40 395 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 396 }
ftppr 0:1bf7e0755f40 397
ftppr 0:1bf7e0755f40 398 // -----------R3------------
ftppr 0:1bf7e0755f40 399 // x
ftppr 0:1bf7e0755f40 400 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0)
ftppr 0:1bf7e0755f40 401 {
ftppr 0:1bf7e0755f40 402 strcpy(msg + len, "R3: direction is x(horizontal) and the value is ");
ftppr 0:1bf7e0755f40 403 len = strlen(msg);
ftppr 0:1bf7e0755f40 404 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 405 }
ftppr 0:1bf7e0755f40 406 // y
ftppr 0:1bf7e0755f40 407 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1)
ftppr 0:1bf7e0755f40 408 {
ftppr 0:1bf7e0755f40 409 strcpy(msg + len, "R3: direction is y(vertical) and the value is ");
ftppr 0:1bf7e0755f40 410 len = strlen(msg);
ftppr 0:1bf7e0755f40 411 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 412 }
ftppr 0:1bf7e0755f40 413
ftppr 0:1bf7e0755f40 414 // -----------L2------------
ftppr 0:1bf7e0755f40 415 if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1)
ftppr 0:1bf7e0755f40 416 {
ftppr 0:1bf7e0755f40 417 strcpy(msg + len, "L2: the value is ");
ftppr 0:1bf7e0755f40 418 len = strlen(msg);
ftppr 0:1bf7e0755f40 419 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 0:1bf7e0755f40 420 }
ftppr 0:1bf7e0755f40 421 // -----------R2------------
ftppr 0:1bf7e0755f40 422 if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1)
ftppr 0:1bf7e0755f40 423 {
ftppr 0:1bf7e0755f40 424 strcpy(msg + len, "L3: the value is ");
ftppr 0:1bf7e0755f40 425 len = strlen(msg);
ftppr 0:1bf7e0755f40 426 itoa(joystick_msg.data[2], msg + len, 10);
ftppr 2:e570143bf35e 427
ftppr 0:1bf7e0755f40 428 }
ftppr 0:1bf7e0755f40 429
ftppr 0:1bf7e0755f40 430 responser.publish(&str_msg);
ftppr 0:1bf7e0755f40 431 }