Robocon 2021
MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
main.cpp@6:0b79b90b83a2, 2021-06-16 (annotated)
- Committer:
- ftppr
- Date:
- Wed Jun 16 10:10:57 2021 +0000
- Revision:
- 6:0b79b90b83a2
- Parent:
- 5:03af248f3f7c
- Child:
- 7:66c6e14a1d97
- Child:
- 8:9abe58bd7c07
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 | 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 | { |
| ftppr | 6:0b79b90b83a2 | 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++){ |
| ftppr | 6:0b79b90b83a2 | 102 | outValue[i]/8; |
| ftppr | 6:0b79b90b83a2 | 103 | (outValue[i]/4)%2; |
| ftppr | 6:0b79b90b83a2 | 104 | (outValue[i]/2 |
| ftppr | 6:0b79b90b83a2 | 105 | (outValue[i]%2; |
| ftppr | 6:0b79b90b83a2 | 106 | } |
| ftppr | 6:0b79b90b83a2 | 107 | } |
| ftppr | 5:03af248f3f7c | 108 | } |
| ftppr | 6:0b79b90b83a2 | 109 | |
| ftppr | 5:03af248f3f7c | 110 | } |
| ftppr | 2:e570143bf35e | 111 | |
| ftppr | 2:e570143bf35e | 112 | // button_msg: Int8 |
| ftppr | 2:e570143bf35e | 113 | void msgCb_button(const std_msgs::Int8 &button_msg) |
| ftppr | 2:e570143bf35e | 114 | { |
| ftppr | 2:e570143bf35e | 115 | memset(msg, 0, sizeof(msg)); |
| ftppr | 2:e570143bf35e | 116 | strcpy(msg, "Received "); |
| ftppr | 2:e570143bf35e | 117 | itoa(button_msg.data, msg + 9, 10); |
| ftppr | 2:e570143bf35e | 118 | int len = strlen(msg); |
| ftppr | 2:e570143bf35e | 119 | msg[len++] = ';'; |
| ftppr | 2:e570143bf35e | 120 | msg[len++] = ' '; |
| ftppr | 2:e570143bf35e | 121 | switch (button_msg.data) |
| ftppr | 2:e570143bf35e | 122 | { |
| ftppr | 2:e570143bf35e | 123 | case 1: |
| ftppr | 2:e570143bf35e | 124 | eject_1 = 1 - eject_1; |
| ftppr | 2:e570143bf35e | 125 | eject_2 = 1 - eject_2; |
| ftppr | 2:e570143bf35e | 126 | strcpy(msg + len, "x is pressed, eject_1 is "); |
| ftppr | 2:e570143bf35e | 127 | msg[strlen(msg)] = '0' + int(eject_1); |
| ftppr | 5:03af248f3f7c | 128 | // ---------- added for clearing n_loaded ------------- |
| ftppr | 5:03af248f3f7c | 129 | n_loaded = 6; |
| ftppr | 5:03af248f3f7c | 130 | strcpy(msg + strlen(msg), "n_loaded is "); |
| ftppr | 5:03af248f3f7c | 131 | msg[strlen(msg)] = '0' + n_loaded; |
| ftppr | 5:03af248f3f7c | 132 | // ---------- end -------------- |
| ftppr | 2:e570143bf35e | 133 | break; |
| ftppr | 2:e570143bf35e | 134 | case -1: |
| ftppr | 2:e570143bf35e | 135 | strcpy(msg + len, "x is released, eject_2 is "); |
| ftppr | 2:e570143bf35e | 136 | msg[strlen(msg)] = '0' + int(eject_2); |
| ftppr | 2:e570143bf35e | 137 | break; |
| ftppr | 2:e570143bf35e | 138 | case 2: |
| ftppr | 2:e570143bf35e | 139 | lock = 1 - lock; |
| ftppr | 2:e570143bf35e | 140 | strcpy(msg + len, "o is pressed, lock is: "); |
| ftppr | 2:e570143bf35e | 141 | msg[strlen(msg)] = '0' + int(lock); |
| ftppr | 2:e570143bf35e | 142 | |
| ftppr | 2:e570143bf35e | 143 | break; |
| ftppr | 2:e570143bf35e | 144 | case -2: |
| ftppr | 2:e570143bf35e | 145 | strcpy(msg + len, "o is released"); |
| ftppr | 2:e570143bf35e | 146 | break; |
| ftppr | 2:e570143bf35e | 147 | case 3: |
| ftppr | 5:03af248f3f7c | 148 | pin3 = 1; |
| ftppr | 5:03af248f3f7c | 149 | strcpy(msg + len, "triangle is pressed, pin3 is "); |
| ftppr | 5:03af248f3f7c | 150 | msg[strlen(msg)] = '0' + int(pin3); |
| ftppr | 2:e570143bf35e | 151 | break; |
| ftppr | 2:e570143bf35e | 152 | case -3: |
| ftppr | 5:03af248f3f7c | 153 | pin3 = 0; |
| ftppr | 5:03af248f3f7c | 154 | strcpy(msg + len, "triangle is released, pin3 is "); |
| ftppr | 5:03af248f3f7c | 155 | msg[strlen(msg)] = '0' + int(pin3); |
| ftppr | 2:e570143bf35e | 156 | break; |
| ftppr | 2:e570143bf35e | 157 | case 4: |
| ftppr | 2:e570143bf35e | 158 | strcpy(msg + len, "square is pressed"); |
| ftppr | 5:03af248f3f7c | 159 | pin1 = 1; pin2 = 1; pin3 = 0; lock = 1; |
| ftppr | 2:e570143bf35e | 160 | break; |
| ftppr | 2:e570143bf35e | 161 | case -4: |
| ftppr | 2:e570143bf35e | 162 | strcpy(msg + len, "square is released"); |
| ftppr | 5:03af248f3f7c | 163 | pin1 = 0; pin2 = 0; |
| ftppr | 2:e570143bf35e | 164 | break; |
| ftppr | 2:e570143bf35e | 165 | case 5: |
| ftppr | 5:03af248f3f7c | 166 | |
| ftppr | 5:03af248f3f7c | 167 | pin1 = 0; |
| ftppr | 5:03af248f3f7c | 168 | pin3 = 0; |
| ftppr | 5:03af248f3f7c | 169 | pin2 = 1; |
| ftppr | 5:03af248f3f7c | 170 | strcpy(msg + len, "down arrow is pressed, pin2 is "); |
| ftppr | 5:03af248f3f7c | 171 | msg[strlen(msg)] = '0' + int(pin2); |
| ftppr | 2:e570143bf35e | 172 | break; |
| ftppr | 2:e570143bf35e | 173 | case -5: |
| ftppr | 5:03af248f3f7c | 174 | pin2 = 0; pin4 = 0; |
| ftppr | 5:03af248f3f7c | 175 | strcpy(msg + len, "down arrow is released, pin2 is "); |
| ftppr | 5:03af248f3f7c | 176 | msg[strlen(msg)] = '0' + int(pin2); |
| ftppr | 2:e570143bf35e | 177 | break; |
| ftppr | 2:e570143bf35e | 178 | case 6: |
| ftppr | 2:e570143bf35e | 179 | load_manual = 0; |
| ftppr | 2:e570143bf35e | 180 | strcpy(msg + len, "right arrow is pressed. "); |
| ftppr | 2:e570143bf35e | 181 | msg[strlen(msg)] = '0' + n_loaded; |
| ftppr | 2:e570143bf35e | 182 | dc_motor1.set_out(0,1); |
| ftppr | 2:e570143bf35e | 183 | n_loaded++; |
| ftppr | 3:2441bcef5885 | 184 | while(firing2 == 0); |
| ftppr | 4:d5502eb8adcd | 185 | |
| ftppr | 4:d5502eb8adcd | 186 | if(n_loaded <= 5) |
| ftppr | 4:d5502eb8adcd | 187 | { |
| ftppr | 5:03af248f3f7c | 188 | dc_motor1.set_out(0,0.5); |
| ftppr | 5:03af248f3f7c | 189 | wait(1.7); |
| ftppr | 4:d5502eb8adcd | 190 | dc_motor1.set_out(0,0); |
| ftppr | 4:d5502eb8adcd | 191 | } |
| ftppr | 4:d5502eb8adcd | 192 | else |
| ftppr | 4:d5502eb8adcd | 193 | { |
| ftppr | 4:d5502eb8adcd | 194 | load_manual = 1; |
| ftppr | 4:d5502eb8adcd | 195 | strcpy(msg + len, "right arrow is pressed. "); |
| ftppr | 4:d5502eb8adcd | 196 | strcpy(msg + strlen(msg), "n_loaded cleared!"); |
| ftppr | 5:03af248f3f7c | 197 | n_loaded = 0; |
| ftppr | 4:d5502eb8adcd | 198 | } |
| ftppr | 4:d5502eb8adcd | 199 | |
| ftppr | 2:e570143bf35e | 200 | break; |
| ftppr | 6:0b79b90b83a2 | 201 | case -6: |
| ftppr | 2:e570143bf35e | 202 | strcpy(msg + len, "right arrow is released. "); |
| ftppr | 2:e570143bf35e | 203 | break; |
| ftppr | 6:0b79b90b83a2 | 204 | case 7: |
| ftppr | 5:03af248f3f7c | 205 | pin2 = 0; |
| ftppr | 5:03af248f3f7c | 206 | pin1 = 1; |
| ftppr | 5:03af248f3f7c | 207 | strcpy(msg + len, "up arrow is pressed, pin1 is"); |
| ftppr | 5:03af248f3f7c | 208 | msg[strlen(msg)] = '0' + int(pin1); |
| ftppr | 2:e570143bf35e | 209 | break; |
| ftppr | 2:e570143bf35e | 210 | case -7: |
| ftppr | 6:0b79b90b83a2 | 211 | pin1 = 0; |
| ftppr | 5:03af248f3f7c | 212 | strcpy(msg + len, "up arrow is released, pin1 is"); |
| ftppr | 5:03af248f3f7c | 213 | msg[strlen(msg)] = '0' + int(pin1); |
| ftppr | 2:e570143bf35e | 214 | break; |
| ftppr | 2:e570143bf35e | 215 | case 8: |
| ftppr | 6:0b79b90b83a2 | 216 | strcpy(msg + len, "left arrow is pressed, phase is "); |
| ftppr | 2:e570143bf35e | 217 | msg[strlen(msg)] = '0' + stage_pick; |
| ftppr | 2:e570143bf35e | 218 | if(stage_pick == 0) |
| ftppr | 2:e570143bf35e | 219 | { |
| ftppr | 6:0b79b90b83a2 | 220 | dc_motor2.set_out(0, 1); |
| ftppr | 6:0b79b90b83a2 | 221 | // if(firing3 == 1) |
| ftppr | 6:0b79b90b83a2 | 222 | // { |
| ftppr | 6:0b79b90b83a2 | 223 | // dc_motor2.set_out(0,0); |
| ftppr | 6:0b79b90b83a2 | 224 | // strcpy(msg + strlen(msg), ", firing pin detected. "); |
| ftppr | 6:0b79b90b83a2 | 225 | // } |
| ftppr | 2:e570143bf35e | 226 | stage_pick++; |
| ftppr | 2:e570143bf35e | 227 | eject_1 = 1; eject_2 = 0; |
| ftppr | 6:0b79b90b83a2 | 228 | strcpy(msg + strlen(msg), "rotate out"); |
| ftppr | 2:e570143bf35e | 229 | } |
| ftppr | 2:e570143bf35e | 230 | else if(stage_pick == 1) |
| ftppr | 2:e570143bf35e | 231 | { |
| ftppr | 2:e570143bf35e | 232 | dc_motor2.set_out(0.5,0); |
| ftppr | 2:e570143bf35e | 233 | wait(0.3); |
| ftppr | 2:e570143bf35e | 234 | dc_motor2.set_out(0,0); |
| ftppr | 2:e570143bf35e | 235 | stage_pick++; |
| ftppr | 6:0b79b90b83a2 | 236 | strcpy(msg + strlen(msg), "rotate phase 1"); |
| ftppr | 2:e570143bf35e | 237 | } |
| ftppr | 2:e570143bf35e | 238 | else if(stage_pick == 2) |
| ftppr | 2:e570143bf35e | 239 | { |
| ftppr | 2:e570143bf35e | 240 | dc_motor2.set_out(0.5,0); |
| ftppr | 2:e570143bf35e | 241 | wait(0.3); |
| ftppr | 2:e570143bf35e | 242 | dc_motor2.set_out(0,0); |
| ftppr | 2:e570143bf35e | 243 | stage_pick = 0; |
| ftppr | 6:0b79b90b83a2 | 244 | strcpy(msg + strlen(msg), "rotate phase 2"); |
| ftppr | 2:e570143bf35e | 245 | } |
| ftppr | 2:e570143bf35e | 246 | break; |
| ftppr | 2:e570143bf35e | 247 | case -8: |
| ftppr | 6:0b79b90b83a2 | 248 | // strcpy(msg + len, "left arrow is released, left is "); |
| ftppr | 2:e570143bf35e | 249 | break; |
| ftppr | 2:e570143bf35e | 250 | case 9: |
| ftppr | 6:0b79b90b83a2 | 251 | strcpy(msg + len, "L1 is pressed"); |
| ftppr | 6:0b79b90b83a2 | 252 | // msg[strlen(msg)] = '0' + auto_shooting; |
| ftppr | 2:e570143bf35e | 253 | break; |
| ftppr | 2:e570143bf35e | 254 | case -9: |
| ftppr | 6:0b79b90b83a2 | 255 | // pin3 = 0; pin4 = 0; |
| ftppr | 2:e570143bf35e | 256 | strcpy(msg + len, "L1 is released"); |
| ftppr | 2:e570143bf35e | 257 | break; |
| ftppr | 2:e570143bf35e | 258 | case 10: |
| ftppr | 6:0b79b90b83a2 | 259 | |
| ftppr | 2:e570143bf35e | 260 | dc_motor1.set_out(0,0); |
| ftppr | 2:e570143bf35e | 261 | dc_motor2.set_out(0,0); |
| ftppr | 6:0b79b90b83a2 | 262 | auto_shooting = 1; |
| ftppr | 6:0b79b90b83a2 | 263 | pin4 = 1; |
| ftppr | 6:0b79b90b83a2 | 264 | strcpy(msg + len, "R1 is pressed, auto_shooting is "); |
| ftppr | 6:0b79b90b83a2 | 265 | msg[strlen(msg)] = '0' + auto_shooting; |
| ftppr | 2:e570143bf35e | 266 | break; |
| ftppr | 2:e570143bf35e | 267 | case -10: |
| ftppr | 6:0b79b90b83a2 | 268 | wait(0.1); |
| ftppr | 6:0b79b90b83a2 | 269 | auto_shooting = 0; |
| ftppr | 6:0b79b90b83a2 | 270 | pin4 = 0; |
| ftppr | 2:e570143bf35e | 271 | strcpy(msg + len, "R1 is released"); |
| ftppr | 2:e570143bf35e | 272 | break; |
| ftppr | 2:e570143bf35e | 273 | case 11: |
| ftppr | 6:0b79b90b83a2 | 274 | strcpy(msg + len, "share is pressed"); |
| ftppr | 2:e570143bf35e | 275 | break; |
| ftppr | 2:e570143bf35e | 276 | case -11: |
| ftppr | 2:e570143bf35e | 277 | strcpy(msg + len, "share is released"); |
| ftppr | 2:e570143bf35e | 278 | break; |
| ftppr | 2:e570143bf35e | 279 | case 12: |
| ftppr | 2:e570143bf35e | 280 | strcpy(msg + len, "option is pressed"); |
| ftppr | 2:e570143bf35e | 281 | break; |
| ftppr | 2:e570143bf35e | 282 | case -12: |
| ftppr | 2:e570143bf35e | 283 | strcpy(msg + len, "option is released"); |
| ftppr | 2:e570143bf35e | 284 | break; |
| ftppr | 2:e570143bf35e | 285 | case 13: |
| ftppr | 5:03af248f3f7c | 286 | pin1 = 1; pin2 = 1; pin3 = 1; |
| ftppr | 2:e570143bf35e | 287 | strcpy(msg + len, "playstation button is pressed, cur pos should be saved"); |
| ftppr | 2:e570143bf35e | 288 | break; |
| ftppr | 2:e570143bf35e | 289 | case -13: |
| ftppr | 5:03af248f3f7c | 290 | pin1 = 0; pin2 = 0; pin3= 0; |
| ftppr | 2:e570143bf35e | 291 | strcpy(msg + len, "playstation button is released"); |
| ftppr | 2:e570143bf35e | 292 | break; |
| ftppr | 2:e570143bf35e | 293 | default: |
| ftppr | 2:e570143bf35e | 294 | len = strlen(msg); |
| ftppr | 2:e570143bf35e | 295 | strcpy(msg + len, "Unrecognized pattern"); |
| ftppr | 2:e570143bf35e | 296 | break; |
| ftppr | 2:e570143bf35e | 297 | } |
| ftppr | 2:e570143bf35e | 298 | |
| ftppr | 2:e570143bf35e | 299 | responser.publish(&str_msg); |
| ftppr | 2:e570143bf35e | 300 | } |
| ftppr | 2:e570143bf35e | 301 | |
| ftppr | 2:e570143bf35e | 302 | |
| ftppr | 2:e570143bf35e | 303 | // --------------- main ----------------- |
| ftppr | 2:e570143bf35e | 304 | int main() |
| ftppr | 2:e570143bf35e | 305 | { |
| ftppr | 2:e570143bf35e | 306 | |
| ftppr | 2:e570143bf35e | 307 | // -> Set Baud Rate |
| ftppr | 2:e570143bf35e | 308 | nh.getHardware()->setBaud(BAUD_RATE); |
| ftppr | 2:e570143bf35e | 309 | nh.initNode(); |
| ftppr | 2:e570143bf35e | 310 | // Advertise publishers |
| ftppr | 2:e570143bf35e | 311 | nh.advertise(responser); |
| ftppr | 5:03af248f3f7c | 312 | nh.advertise(responser2); |
| ftppr | 2:e570143bf35e | 313 | // Initiate display |
| ftppr | 5:03af248f3f7c | 314 | // msg = "------- LAST DAY -------"; |
| ftppr | 2:e570143bf35e | 315 | str_msg.data = msg; |
| ftppr | 5:03af248f3f7c | 316 | str_msg2.data = msg2; |
| ftppr | 2:e570143bf35e | 317 | // responser.publish(&str_msg); |
| ftppr | 5:03af248f3f7c | 318 | // responser2.publish(&str_msg2); |
| ftppr | 2:e570143bf35e | 319 | // Initiate subscribers |
| ftppr | 2:e570143bf35e | 320 | nh.subscribe(sub_button); |
| ftppr | 2:e570143bf35e | 321 | nh.subscribe(sub_joystick); |
| ftppr | 5:03af248f3f7c | 322 | nh.subscribe(sub_cam); |
| ftppr | 2:e570143bf35e | 323 | // Initiate IOs, keep everything at rest |
| ftppr | 2:e570143bf35e | 324 | myled = 0; |
| ftppr | 5:03af248f3f7c | 325 | pin1 = 0;pin2 = 0; pin3 = 0; lock = 0; |
| ftppr | 2:e570143bf35e | 326 | eject_1 = 0; eject_2 = 1; |
| ftppr | 2:e570143bf35e | 327 | |
| ftppr | 2:e570143bf35e | 328 | // Initiate firing pins |
| ftppr | 5:03af248f3f7c | 329 | firing1.mode(PullUp); // default 0 |
| ftppr | 6:0b79b90b83a2 | 330 | firing2.mode(PullUp); // default 0 |
| ftppr | 6:0b79b90b83a2 | 331 | firing3.mode(PullUp); // default 0 |
| ftppr | 2:e570143bf35e | 332 | |
| ftppr | 2:e570143bf35e | 333 | // Reset dc motor |
| ftppr | 2:e570143bf35e | 334 | dc_motor1.reset(); |
| ftppr | 5:03af248f3f7c | 335 | // dc_motor1.set_pid(0.008, 0, 0, 0.0); |
| ftppr | 2:e570143bf35e | 336 | dc_motor2.reset(); |
| ftppr | 2:e570143bf35e | 337 | dc_motor2.set_pid(0.008, 0, 0, 0.0); |
| ftppr | 5:03af248f3f7c | 338 | // up = 1; pin2 = 1; pin2 = 0; |
| ftppr | 6:0b79b90b83a2 | 339 | // aout.period_us(0.01); |
| ftppr | 6:0b79b90b83a2 | 340 | aout = 0; |
| ftppr | 2:e570143bf35e | 341 | // Main programming |
| ftppr | 2:e570143bf35e | 342 | int start = 0; // control variable for resetting shooter |
| ftppr | 2:e570143bf35e | 343 | int tmp = 0; |
| ftppr | 2:e570143bf35e | 344 | |
| ftppr | 2:e570143bf35e | 345 | while(true) { |
| ftppr | 2:e570143bf35e | 346 | nh.spinOnce(); |
| ftppr | 2:e570143bf35e | 347 | |
| ftppr | 2:e570143bf35e | 348 | if(nh.connected()) |
| ftppr | 2:e570143bf35e | 349 | myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson |
| ftppr | 2:e570143bf35e | 350 | else |
| ftppr | 2:e570143bf35e | 351 | myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson |
| ftppr | 2:e570143bf35e | 352 | if(button == 1) |
| ftppr | 2:e570143bf35e | 353 | { |
| ftppr | 2:e570143bf35e | 354 | myled = 0; |
| ftppr | 3:2441bcef5885 | 355 | if(!faul_reset && firing1 == 0) |
| ftppr | 2:e570143bf35e | 356 | { |
| ftppr | 6:0b79b90b83a2 | 357 | pin2 = 0; pin1 = 1; pin3 = 1; // 101 |
| ftppr | 2:e570143bf35e | 358 | start = 1; |
| ftppr | 4:d5502eb8adcd | 359 | memset(msg, 0, sizeof(msg)); |
| ftppr | 4:d5502eb8adcd | 360 | strcpy(msg, "Start resetting Faulhaber: firing1 is "); |
| ftppr | 4:d5502eb8adcd | 361 | msg[strlen(msg)] = '0' + firing1.read(); |
| ftppr | 4:d5502eb8adcd | 362 | responser.publish(&str_msg); |
| ftppr | 4:d5502eb8adcd | 363 | // faul_reset = 1; |
| ftppr | 2:e570143bf35e | 364 | } |
| ftppr | 2:e570143bf35e | 365 | else |
| ftppr | 2:e570143bf35e | 366 | { |
| ftppr | 3:2441bcef5885 | 367 | dc_motor1.set_out(0,1); |
| ftppr | 3:2441bcef5885 | 368 | load_manual = 1; |
| ftppr | 2:e570143bf35e | 369 | } |
| ftppr | 2:e570143bf35e | 370 | while(button == 1); |
| ftppr | 2:e570143bf35e | 371 | wait(0.2); |
| ftppr | 2:e570143bf35e | 372 | memset(msg, 0, sizeof(msg)); |
| ftppr | 2:e570143bf35e | 373 | strcpy(msg, "user button is pressed, faul_reset is "); |
| ftppr | 3:2441bcef5885 | 374 | msg[strlen(msg)] = '0' + faul_reset; |
| ftppr | 3:2441bcef5885 | 375 | strcpy(msg + strlen(msg), ", firing2 is "); |
| ftppr | 3:2441bcef5885 | 376 | msg[strlen(msg)] = '0' + firing2.read(); |
| ftppr | 3:2441bcef5885 | 377 | strcpy(msg + strlen(msg), ", load_manual is "); |
| ftppr | 3:2441bcef5885 | 378 | msg[strlen(msg)] = '0' + load_manual; |
| ftppr | 2:e570143bf35e | 379 | responser.publish(&str_msg); |
| ftppr | 2:e570143bf35e | 380 | } |
| ftppr | 2:e570143bf35e | 381 | if(load_manual && firing2 == 1) |
| ftppr | 2:e570143bf35e | 382 | { |
| ftppr | 3:2441bcef5885 | 383 | wait(0.17); |
| ftppr | 3:2441bcef5885 | 384 | dc_motor1.set_out(0,0); |
| ftppr | 3:2441bcef5885 | 385 | memset(msg, 0, sizeof(msg)); |
| ftppr | 3:2441bcef5885 | 386 | strcpy(msg, "firing2 is touched: "); |
| ftppr | 3:2441bcef5885 | 387 | msg[strlen(msg)] = '0' + firing2.read(); |
| ftppr | 2:e570143bf35e | 388 | } |
| ftppr | 3:2441bcef5885 | 389 | if(firing1.read() == 1 && start == 1) |
| ftppr | 2:e570143bf35e | 390 | { |
| ftppr | 5:03af248f3f7c | 391 | pin1 = 0; pin2 = 1; pin3 = 1; // 110 |
| ftppr | 2:e570143bf35e | 392 | wait(0.3); |
| ftppr | 2:e570143bf35e | 393 | memset(msg, 0, sizeof(msg)); |
| ftppr | 2:e570143bf35e | 394 | strcpy(msg, "firing1 is touched: "); |
| ftppr | 2:e570143bf35e | 395 | msg[strlen(msg)] = '0' + firing1.read(); |
| ftppr | 2:e570143bf35e | 396 | responser.publish(&str_msg); |
| ftppr | 5:03af248f3f7c | 397 | pin3 = 0; pin2 = 0; pin1 = 0; |
| ftppr | 3:2441bcef5885 | 398 | start = 0; |
| ftppr | 2:e570143bf35e | 399 | |
| ftppr | 2:e570143bf35e | 400 | } |
| ftppr | 2:e570143bf35e | 401 | } |
| ftppr | 2:e570143bf35e | 402 | } |
| ftppr | 0:1bf7e0755f40 | 403 | |
| ftppr | 0:1bf7e0755f40 | 404 | |
| ftppr | 0:1bf7e0755f40 | 405 | void itoa(int num, char *str, int radix) |
| ftppr | 0:1bf7e0755f40 | 406 | { /*索引表*/ |
| ftppr | 0:1bf7e0755f40 | 407 | char index[] = "0123456789ABCDEF"; |
| ftppr | 0:1bf7e0755f40 | 408 | unsigned unum; /*中间变量*/ |
| ftppr | 0:1bf7e0755f40 | 409 | int i = 0, j, k; |
| ftppr | 0:1bf7e0755f40 | 410 | /*确定unum的值*/ |
| ftppr | 0:1bf7e0755f40 | 411 | if (radix == 10 && num < 0) /*十进制负数*/ |
| ftppr | 0:1bf7e0755f40 | 412 | { |
| ftppr | 0:1bf7e0755f40 | 413 | unum = (unsigned)-num; |
| ftppr | 0:1bf7e0755f40 | 414 | str[i++] = '-'; |
| ftppr | 0:1bf7e0755f40 | 415 | } |
| ftppr | 0:1bf7e0755f40 | 416 | else |
| ftppr | 0:1bf7e0755f40 | 417 | unum = (unsigned)num; /*其他情况*/ |
| ftppr | 0:1bf7e0755f40 | 418 | /*转换*/ |
| ftppr | 0:1bf7e0755f40 | 419 | do |
| ftppr | 0:1bf7e0755f40 | 420 | { |
| ftppr | 0:1bf7e0755f40 | 421 | str[i++] = index[unum % (unsigned)radix]; |
| ftppr | 0:1bf7e0755f40 | 422 | unum /= radix; |
| ftppr | 0:1bf7e0755f40 | 423 | } while (unum); |
| ftppr | 0:1bf7e0755f40 | 424 | str[i] = '\0'; |
| ftppr | 0:1bf7e0755f40 | 425 | /*逆序*/ |
| ftppr | 0:1bf7e0755f40 | 426 | if (str[0] == '-') |
| ftppr | 0:1bf7e0755f40 | 427 | k = 1; /*十进制负数*/ |
| ftppr | 0:1bf7e0755f40 | 428 | else |
| ftppr | 0:1bf7e0755f40 | 429 | k = 0; |
| ftppr | 0:1bf7e0755f40 | 430 | |
| ftppr | 0:1bf7e0755f40 | 431 | for (j = k; j <= (i - 1) / 2; j++) |
| ftppr | 0:1bf7e0755f40 | 432 | { |
| ftppr | 0:1bf7e0755f40 | 433 | char temp; |
| ftppr | 0:1bf7e0755f40 | 434 | temp = str[j]; |
| ftppr | 0:1bf7e0755f40 | 435 | str[j] = str[i - 1 + k - j]; |
| ftppr | 0:1bf7e0755f40 | 436 | str[i - 1 + k - j] = temp; |
| ftppr | 0:1bf7e0755f40 | 437 | } |
| ftppr | 0:1bf7e0755f40 | 438 | } |
| ftppr | 0:1bf7e0755f40 | 439 | |
| ftppr | 0:1bf7e0755f40 | 440 | /* |
| ftppr | 0:1bf7e0755f40 | 441 | * joystick_msg: (Left/Right, x/y, value) |
| ftppr | 0:1bf7e0755f40 | 442 | */ |
| ftppr | 0:1bf7e0755f40 | 443 | |
| ftppr | 0:1bf7e0755f40 | 444 | void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg) |
| ftppr | 0:1bf7e0755f40 | 445 | { |
| ftppr | 0:1bf7e0755f40 | 446 | // myled = !myled; // blink the led |
| ftppr | 0:1bf7e0755f40 | 447 | memset(msg, 0, sizeof(msg)); |
| ftppr | 0:1bf7e0755f40 | 448 | // eg. Received (0,0,-100) |
| ftppr | 0:1bf7e0755f40 | 449 | strcpy(msg, "Received ("); |
| ftppr | 0:1bf7e0755f40 | 450 | int len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 451 | msg[len++] = '0' + joystick_msg.data[0]; |
| ftppr | 0:1bf7e0755f40 | 452 | msg[len++] = ','; |
| ftppr | 0:1bf7e0755f40 | 453 | itoa(joystick_msg.data[1], msg + len, 10); |
| ftppr | 0:1bf7e0755f40 | 454 | len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 455 | msg[len++] = ','; |
| ftppr | 0:1bf7e0755f40 | 456 | itoa(joystick_msg.data[2], msg + len, 10); |
| ftppr | 0:1bf7e0755f40 | 457 | len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 458 | msg[len++] = ')'; |
| ftppr | 0:1bf7e0755f40 | 459 | msg[len++] = ';'; |
| ftppr | 0:1bf7e0755f40 | 460 | msg[len++] = ' '; |
| ftppr | 0:1bf7e0755f40 | 461 | |
| ftppr | 0:1bf7e0755f40 | 462 | // Elaboration of the message |
| ftppr | 0:1bf7e0755f40 | 463 | // -----------L3------------ |
| ftppr | 0:1bf7e0755f40 | 464 | // x |
| ftppr | 0:1bf7e0755f40 | 465 | if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0) |
| ftppr | 0:1bf7e0755f40 | 466 | { |
| ftppr | 0:1bf7e0755f40 | 467 | strcpy(msg + len, "L3: direction is x(horizontal) and the value is "); |
| ftppr | 0:1bf7e0755f40 | 468 | len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 469 | itoa(joystick_msg.data[2], msg + len, 10); |
| ftppr | 0:1bf7e0755f40 | 470 | } |
| ftppr | 0:1bf7e0755f40 | 471 | // y |
| ftppr | 0:1bf7e0755f40 | 472 | if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1) |
| ftppr | 0:1bf7e0755f40 | 473 | { |
| ftppr | 0:1bf7e0755f40 | 474 | strcpy(msg + len, "L3: direction is y(vertical) 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 | |
| ftppr | 0:1bf7e0755f40 | 479 | // -----------R3------------ |
| ftppr | 0:1bf7e0755f40 | 480 | // x |
| ftppr | 0:1bf7e0755f40 | 481 | if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0) |
| ftppr | 0:1bf7e0755f40 | 482 | { |
| ftppr | 0:1bf7e0755f40 | 483 | strcpy(msg + len, "R3: direction is x(horizontal) and the value is "); |
| ftppr | 0:1bf7e0755f40 | 484 | len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 485 | itoa(joystick_msg.data[2], msg + len, 10); |
| ftppr | 0:1bf7e0755f40 | 486 | } |
| ftppr | 0:1bf7e0755f40 | 487 | // y |
| ftppr | 0:1bf7e0755f40 | 488 | if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1) |
| ftppr | 0:1bf7e0755f40 | 489 | { |
| ftppr | 0:1bf7e0755f40 | 490 | strcpy(msg + len, "R3: direction is y(vertical) 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 | |
| ftppr | 0:1bf7e0755f40 | 495 | // -----------L2------------ |
| ftppr | 0:1bf7e0755f40 | 496 | if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1) |
| ftppr | 0:1bf7e0755f40 | 497 | { |
| ftppr | 0:1bf7e0755f40 | 498 | strcpy(msg + len, "L2: the value is "); |
| ftppr | 0:1bf7e0755f40 | 499 | len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 500 | itoa(joystick_msg.data[2], msg + len, 10); |
| ftppr | 0:1bf7e0755f40 | 501 | } |
| ftppr | 0:1bf7e0755f40 | 502 | // -----------R2------------ |
| ftppr | 0:1bf7e0755f40 | 503 | if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1) |
| ftppr | 0:1bf7e0755f40 | 504 | { |
| ftppr | 0:1bf7e0755f40 | 505 | strcpy(msg + len, "L3: the value is "); |
| ftppr | 0:1bf7e0755f40 | 506 | len = strlen(msg); |
| ftppr | 0:1bf7e0755f40 | 507 | itoa(joystick_msg.data[2], msg + len, 10); |
| ftppr | 2:e570143bf35e | 508 | |
| ftppr | 0:1bf7e0755f40 | 509 | } |
| ftppr | 0:1bf7e0755f40 | 510 | |
| ftppr | 0:1bf7e0755f40 | 511 | responser.publish(&str_msg); |
| ftppr | 0:1bf7e0755f40 | 512 | } |