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