![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 2:e570143bf35e
- Parent:
- 1:fbdbf4ac12c6
- Child:
- 3:2441bcef5885
--- a/main.cpp Wed Jun 02 13:53:04 2021 +0000 +++ b/main.cpp Wed Jun 09 02:39:27 2021 +0000 @@ -1,4 +1,3 @@ - // Hello World to sweep a servo through its full range // ROS Connect Baud Rate #define BAUD_RATE 115200 @@ -8,33 +7,310 @@ #include <std_msgs/Int8.h> #include <std_msgs/String.h> #include <std_msgs/Int16MultiArray.h> -ros::NodeHandle nh; -DigitalOut myled(LED1); +#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); -char msg[200] = {0}; +// ============ Define IOs ============= +// External Button DigitalIn button(USER_BUTTON); - -DigitalOut up = D12; // pin 1 of faulhaber +// LED +DigitalOut myled(LED1); +// Faulhaber pins +DigitalOut up = A3; // pin 1 of faulhaber DigitalOut down = A5; // pin 2 of faulhaber -DigitalOut left = D2; // DC motor 1 (via L298N control pin 1) -DigitalOut right = D4; // DC motor 2 (via L298N control pin 2) -//AnalogOut left = A0; // DC motor 1 (via L298N control pin 1) -//AnalogOut right = A1; // DC motor 2 (via L298N control pin 2) -DigitalOut lock = D14; // 電磁石 DigitalOut back = A4; // pin 3 of faulhaber -DigitalOut eject_1 = D10; // valve 1 -DigitalOut eject_2 = D5; // valve 2 -//DigitalOut savePos = D0; savepos by back=1 & up=1 & down=1 -DigitalIn firing = D3; +// 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; 轉夾 +DC_Motor_PID dc_motor2(D12,D13,D4,D7,792); //M1, M2, INA, INB, PPR; load 1 arrow + +// 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; + 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++; + if(firing2 == 1) + { + if(n_loaded <= 5) + { + dc_motor1.set_out(0,0.5); + wait(1.2); + } + else + { + strcpy(msg + len, "right arrow is pressed. "); + strcpy(msg + strlen(msg), "n_loaded cleared!"); + n_loaded = 0; + } + dc_motor1.set_out(0,0); + } +// load_manual = 1; + 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(PullDown); //default 0 + 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); +// dc_motor2.set_out(0,0.5); +// wait(0.3); +// dc_motor2.move_angle(100); + dc_motor2.set_out(0,0); +// dc_motor1.move_angle(100); +// dc_motor1.set_out(0.5,0); // -> testing only + + // 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) + { + down = 0; up = 1; back = 1; // 101 + start = 1; + faul_reset = 1; + } + else + { + dc_motor1.set_out(0,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; + responser.publish(&str_msg); + } + if(load_manual && firing2 == 1) + { + dc_motor1.set_out(0,0); + } + if(firing1.read() == 0 && start == 1) + { + up = 0; down = 1; back = 1; // 110 + while(firing1.read() == 0); + wait(0.3); + down = 0; back = 0; up = 0; + start = 0; + memset(msg, 0, sizeof(msg)); + strcpy(msg, "firing1 is touched: "); + msg[strlen(msg)] = '0' + firing1.read(); + responser.publish(&str_msg); + + } + } +} void itoa(int num, char *str, int radix) @@ -140,188 +416,8 @@ strcpy(msg + len, "L3: the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); + } responser.publish(&str_msg); -} - -// button_msg: Int8 -void msgCb_button(const std_msgs::Int8 &button_msg) -{ -// myled = !myled; // blink the led - memset(msg, 0, sizeof(msg)); - strcpy(msg, "Received "); - itoa(button_msg.data, msg + 9, 10); - // int len = my_strlen(msg); - 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: -// eject = 0; - 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; - 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: - left = 0; right = 1; - strcpy(msg + len, "right arrow is pressed, right is "); - msg[strlen(msg)] = '0' + int(right); - break; - case -6: - right = 0; - strcpy(msg + len, "right arrow is released, right is "); - msg[strlen(msg)] = '0' + int(right); - 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: - left = 1; right = 0; - strcpy(msg + len, "left arrow is pressed, left is "); - msg[strlen(msg)] = '0' + int(left); - break; - case -8: - left = 0; - strcpy(msg + len, "left arrow is released, left is "); - msg[strlen(msg)] = '0' + int(left); - break; - case 9: - strcpy(msg + len, "L1 is pressed, left is "); - msg[strlen(msg)] = '0' + int(left); - break; - case -9: - strcpy(msg + len, "L1 is released"); - break; - case 10: - 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); -} -int main() -{ - // -> Set Baud Rate - nh.getHardware()->setBaud(BAUD_RATE); - nh.initNode(); - nh.advertise(responser); - str_msg.data = msg; - responser.publish(&str_msg); - nh.subscribe(sub_button); - nh.subscribe(sub_joystick); - myled = 0; - back = 0;up = 0; down = 0; lock = 0; left = 0; right = 0; - eject_1 = 0; eject_2 = 1; - strcpy(msg, "Byebye world."); - responser.publish(&str_msg); - int start = 1; - firing.mode(PullDown); - // Main programming - 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; - down = 0; up = 1; back = 1; - start = 1; - while(button == 1); - } - if(firing.read() == 1 && start == 1) - { - up = 0; - down = 1; - back = 1; - while(firing.read() == 1); - wait(0.3); - down = 0; back = 0; up = 0; - start = 0; - - } - } } \ No newline at end of file