Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@2:e570143bf35e, 2021-06-09 (annotated)

Committer:
ftppr
Date:
Wed Jun 09 02:39:27 2021 +0000
Revision:
2:e570143bf35e
Parent:
1:fbdbf4ac12c6
Child:
3:2441bcef5885
DC_motor tolerance changed from 2 to 5; Add load 1 arrow, tune manually and flipping the catcher

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