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?
User | Revision | Line number | New 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 | } |