Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@5:03af248f3f7c, 2021-06-14 (annotated)

Committer:
ftppr
Date:
Mon Jun 14 09:24:44 2021 +0000
Revision:
5:03af248f3f7c
Parent:
4:d5502eb8adcd
Child:
6:0b79b90b83a2
add a lot of useful and useless stuff

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