Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@7:66c6e14a1d97, 2021-06-16 (annotated)

Committer:
timlee720
Date:
Wed Jun 16 10:36:50 2021 +0000
Revision:
7:66c6e14a1d97
Parent:
6:0b79b90b83a2 
...;

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