Robocon 2021 / Mbed 2 deprecated MBed_TR1

MBed program for TR1 functions

Dependencies:   mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib

main.cpp@8:9abe58bd7c07, 2021-06-18 (annotated)

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