MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
main.cpp
- Committer:
- ftppr
- Date:
- 2021-06-11
- Revision:
- 4:d5502eb8adcd
- Parent:
- 3:2441bcef5885
- Child:
- 5:03af248f3f7c
File content as of revision 4:d5502eb8adcd:
// ROS Connect Baud Rate #define BAUD_RATE 115200 #include "mbed.h" #include "Servo.h" #include <ros.h> #include <std_msgs/Int8.h> #include <std_msgs/String.h> #include <std_msgs/Int16MultiArray.h> #include "DC_Motor_Controller.h" // Declare Functions void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg); void msgCb_button(const std_msgs::Int8 &button_msg); void itoa(int num, char *str, int radix); // Define ros variables ros::NodeHandle nh; // Subs ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick); ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button); // Pubs std_msgs::String str_msg; ros::Publisher responser("display", &str_msg); // ============ Define IOs ============= // External Button DigitalIn button(USER_BUTTON); // LED DigitalOut myled(LED1); // Faulhaber pins DigitalOut up = A3; // pin 1 of faulhaber DigitalOut down = A5; // pin 2 of faulhaber DigitalOut back = A4; // pin 3 of faulhaber // Electromagnet DigitalOut lock = D14; // Firing pin DigitalIn firing1 = D3; // Reset Faulhaber DigitalIn firing2 = D15; // Load 1 arrow DigitalIn firing3 = D2; // Pick, small // Valves DigitalOut eject_1 = A1; // valve 1 DigitalOut eject_2 = A2; // valve 2 // DC motors DC_Motor_PID dc_motor1(D6, D5, D9, D8, 792); //M1, M2, INA, INB, PPR; load 1 arrow DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; picker // Global Control variable char msg[200] = {0}; // msg for responser int stage_pick = 0; // stage of picking up arrows from the rack int n_loaded = 0; // number of arrows. int faul_reset = 0; int load_manual = 1; // tune the loading 1 arrow manually // button_msg: Int8 void msgCb_button(const std_msgs::Int8 &button_msg) { memset(msg, 0, sizeof(msg)); strcpy(msg, "Received "); itoa(button_msg.data, msg + 9, 10); int len = strlen(msg); msg[len++] = ';'; msg[len++] = ' '; switch (button_msg.data) { case 1: eject_1 = 1 - eject_1; eject_2 = 1 - eject_2; strcpy(msg + len, "x is pressed, eject_1 is "); msg[strlen(msg)] = '0' + int(eject_1); break; case -1: strcpy(msg + len, "x is released, eject_2 is "); msg[strlen(msg)] = '0' + int(eject_2); break; case 2: lock = 1 - lock; strcpy(msg + len, "o is pressed, lock is: "); msg[strlen(msg)] = '0' + int(lock); break; case -2: strcpy(msg + len, "o is released"); break; case 3: back = 1; strcpy(msg + len, "triangle is pressed, back is "); msg[strlen(msg)] = '0' + int(back); break; case -3: back = 0; strcpy(msg + len, "triangle is released, back is "); msg[strlen(msg)] = '0' + int(back); break; case 4: strcpy(msg + len, "square is pressed"); up = 1; down = 1; lock = 1; break; case -4: strcpy(msg + len, "square is released"); up = 0; down = 0; break; case 5: down = 1; up = 0; strcpy(msg + len, "down arrow is pressed, down is "); msg[strlen(msg)] = '0' + int(down); break; case -5: down = 0; strcpy(msg + len, "down arrow is released, down is "); msg[strlen(msg)] = '0' + int(down); break; case 6: load_manual = 0; strcpy(msg + len, "right arrow is pressed. "); msg[strlen(msg)] = '0' + n_loaded; dc_motor1.set_out(0,1); n_loaded++; while(firing2 == 0); if(n_loaded <= 5) { dc_motor1.set_out(0,0.7); wait(1.4); dc_motor1.set_out(0,0); } else { load_manual = 1; strcpy(msg + len, "right arrow is pressed. "); strcpy(msg + strlen(msg), "n_loaded cleared!"); n_loaded = 0; } break; case -6: strcpy(msg + len, "right arrow is released. "); break; case 7: up = 1; down = 0; strcpy(msg + len, "up arrow is pressed, up is"); msg[strlen(msg)] = '0' + int(up); break; case -7: up = 0; strcpy(msg + len, "up arrow is released, up is"); msg[strlen(msg)] = '0' + int(up); break; case 8: strcpy(msg + len, "left arrow is pressed, left is "); msg[strlen(msg)] = '0' + stage_pick; if(stage_pick == 0) { dc_motor2.set_out(0, 0.5); if(firing3 == 1) dc_motor2.set_out(0,0); stage_pick++; eject_1 = 1; eject_2 = 0; } else if(stage_pick == 1) { dc_motor2.set_out(0.5,0); wait(0.3); dc_motor2.set_out(0,0); stage_pick++; } else if(stage_pick == 2) { dc_motor2.set_out(0.5,0); wait(0.3); dc_motor2.set_out(0,0); stage_pick = 0; } break; case -8: strcpy(msg + len, "left arrow is released, left is "); break; case 9: strcpy(msg + len, "L1 is pressed"); break; case -9: strcpy(msg + len, "L1 is released"); break; case 10: dc_motor1.set_out(0,0); dc_motor2.set_out(0,0); strcpy(msg + len, "R1 is pressed"); break; case -10: strcpy(msg + len, "R1 is released"); break; case 11: strcpy(msg + len, "share is pressed"); break; case -11: strcpy(msg + len, "share is released"); break; case 12: strcpy(msg + len, "option is pressed"); break; case -12: strcpy(msg + len, "option is released"); break; case 13: up = 1; down = 1; back = 1; strcpy(msg + len, "playstation button is pressed, cur pos should be saved"); break; case -13: up = 0; down = 0; back= 0; strcpy(msg + len, "playstation button is released"); break; default: len = strlen(msg); strcpy(msg + len, "Unrecognized pattern"); break; } responser.publish(&str_msg); } // --------------- main ----------------- int main() { // -> Set Baud Rate nh.getHardware()->setBaud(BAUD_RATE); nh.initNode(); // Advertise publishers nh.advertise(responser); // Initiate display str_msg.data = msg; // responser.publish(&str_msg); // Initiate subscribers nh.subscribe(sub_button); nh.subscribe(sub_joystick); // Initiate IOs, keep everything at rest myled = 0; back = 0;up = 0; down = 0; lock = 0; eject_1 = 0; eject_2 = 1; // Initiate firing pins firing1.mode(PullUp); // default 1 firing2.mode(PullUp); // default 1 firing3.mode(PullUp); // default 1 // Reset dc motor dc_motor1.reset(); dc_motor1.set_pid(0.008, 0, 0, 0.0); dc_motor2.reset(); dc_motor2.set_pid(0.008, 0, 0, 0.0); up = 1; back = 1; down = 0; // Main programming int start = 0; // control variable for resetting shooter int tmp = 0; while(true) { nh.spinOnce(); if(nh.connected()) myled = 1; // Turn on the LED if the ROS successfully connect with the Jetson else myled = 0; // Turn off the LED if the ROS cannot connect with the Jetson if(button == 1) { myled = 0; if(!faul_reset && firing1 == 0) // if(1) { down = 0; up = 1; back = 1; // 101 start = 1; memset(msg, 0, sizeof(msg)); strcpy(msg, "Start resetting Faulhaber: firing1 is "); msg[strlen(msg)] = '0' + firing1.read(); responser.publish(&str_msg); // faul_reset = 1; } else { dc_motor1.set_out(0,1); load_manual = 1; } while(button == 1); wait(0.2); memset(msg, 0, sizeof(msg)); strcpy(msg, "user button is pressed, faul_reset is "); msg[strlen(msg)] = '0' + faul_reset; strcpy(msg + strlen(msg), ", firing2 is "); msg[strlen(msg)] = '0' + firing2.read(); strcpy(msg + strlen(msg), ", load_manual is "); msg[strlen(msg)] = '0' + load_manual; responser.publish(&str_msg); } if(load_manual && firing2 == 1) { wait(0.17); dc_motor1.set_out(0,0); memset(msg, 0, sizeof(msg)); strcpy(msg, "firing2 is touched: "); msg[strlen(msg)] = '0' + firing2.read(); } if(firing1.read() == 1 && start == 1) { up = 0; down = 1; back = 1; // 110 wait(0.3); memset(msg, 0, sizeof(msg)); strcpy(msg, "firing1 is touched: "); msg[strlen(msg)] = '0' + firing1.read(); responser.publish(&str_msg); down = 0; back = 0; up = 0; start = 0; } } } void itoa(int num, char *str, int radix) { /*索引表*/ char index[] = "0123456789ABCDEF"; unsigned unum; /*中间变量*/ int i = 0, j, k; /*确定unum的值*/ if (radix == 10 && num < 0) /*十进制负数*/ { unum = (unsigned)-num; str[i++] = '-'; } else unum = (unsigned)num; /*其他情况*/ /*转换*/ do { str[i++] = index[unum % (unsigned)radix]; unum /= radix; } while (unum); str[i] = '\0'; /*逆序*/ if (str[0] == '-') k = 1; /*十进制负数*/ else k = 0; for (j = k; j <= (i - 1) / 2; j++) { char temp; temp = str[j]; str[j] = str[i - 1 + k - j]; str[i - 1 + k - j] = temp; } } /* * joystick_msg: (Left/Right, x/y, value) */ void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg) { // myled = !myled; // blink the led memset(msg, 0, sizeof(msg)); // eg. Received (0,0,-100) strcpy(msg, "Received ("); int len = strlen(msg); msg[len++] = '0' + joystick_msg.data[0]; msg[len++] = ','; itoa(joystick_msg.data[1], msg + len, 10); len = strlen(msg); msg[len++] = ','; itoa(joystick_msg.data[2], msg + len, 10); len = strlen(msg); msg[len++] = ')'; msg[len++] = ';'; msg[len++] = ' '; // Elaboration of the message // -----------L3------------ // x if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 0) { strcpy(msg + len, "L3: direction is x(horizontal) and the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } // y if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == 1) { strcpy(msg + len, "L3: direction is y(vertical) and the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } // -----------R3------------ // x if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 0) { strcpy(msg + len, "R3: direction is x(horizontal) and the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } // y if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == 1) { strcpy(msg + len, "R3: direction is y(vertical) and the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } // -----------L2------------ if (joystick_msg.data[0] == 0 && joystick_msg.data[1] == -1) { strcpy(msg + len, "L2: the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } // -----------R2------------ if (joystick_msg.data[0] == 1 && joystick_msg.data[1] == -1) { strcpy(msg + len, "L3: the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } responser.publish(&str_msg); }