![](/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:
- 5:03af248f3f7c
- Parent:
- 4:d5502eb8adcd
- Child:
- 6:0b79b90b83a2
--- a/main.cpp Fri Jun 11 02:19:26 2021 +0000 +++ b/main.cpp Mon Jun 14 09:24:44 2021 +0000 @@ -5,13 +5,17 @@ #include "Servo.h" #include <ros.h> #include <std_msgs/Int8.h> +#include <std_msgs/Int16.h> #include <std_msgs/String.h> #include <std_msgs/Int16MultiArray.h> #include "DC_Motor_Controller.h" +//#include "DC_Motor.h" // Declare Functions void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg); void msgCb_button(const std_msgs::Int8 &button_msg); +void msgCam(const std_msgs::Int16 &cam_msg); + void itoa(int num, char *str, int radix); // Define ros variables @@ -19,9 +23,12 @@ // Subs ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick); ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button); +ros::Subscriber<std_msgs::Int16> sub_cam("PotDistance", msgCam); // Pubs std_msgs::String str_msg; +std_msgs::String str_msg2; ros::Publisher responser("display", &str_msg); +ros::Publisher responser2("show_cam", &str_msg2); // ============ Define IOs ============= @@ -30,28 +37,52 @@ // 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 +DigitalOut pin1 = A3; // pin 1 of faulhaber +DigitalOut pin2 = A5; // pin 2 of faulhaber +DigitalOut pin3 = A4; // pin 3 of faulhaber +DigitalOut pin4 = A0; // pin 4 of faulhaber +AnalogOut aout = D13; // Electromagnet DigitalOut lock = D14; // Firing pin -DigitalIn firing1 = D3; // Reset Faulhaber +DigitalIn firing1 = D9; // 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 +DigitalOut eject_1 = D4; // valve 1 +DigitalOut eject_2 = D7; // 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 +DC_Motor_PID dc_motor1(D6, D5, PE_6, PF_8, 792); //M1, M2, INA, INB, PPR; load 1 arrow +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 // Global Control variable char msg[200] = {0}; // msg for responser +char msg2[200] = {0}; // msg for cam show 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 +int auto_shooting = 0; +int smooth = 0; + +// cam callback +void msgCam(const std_msgs::Int16 &cam_msg) +{ + int minimum = 250; + int maximum = 350; +// int data = cam_msg.data; + if(auto_shooting > 0) + { + aout = (cam_msg.data - minimum) / (maximum - minimum); + memset(msg2, 0, sizeof(msg2)); + strcpy(msg2, "Cam distance: "); + itoa(cam_msg.data, msg2 + strlen(msg2), 10); + strcpy(msg2 + strlen(msg2), ", aout(*1000): "); + itoa(int(aout.read()*1000), msg2 + strlen(msg2), 10); +// sprintf("Cam distance: %d, %.4f", data, aout.read()); + responser2.publish(&str_msg2); + } +} // button_msg: Int8 void msgCb_button(const std_msgs::Int8 &button_msg) @@ -62,7 +93,6 @@ int len = strlen(msg); msg[len++] = ';'; msg[len++] = ' '; - switch (button_msg.data) { case 1: @@ -70,6 +100,11 @@ eject_2 = 1 - eject_2; strcpy(msg + len, "x is pressed, eject_1 is "); msg[strlen(msg)] = '0' + int(eject_1); + // ---------- added for clearing n_loaded ------------- + n_loaded = 6; + strcpy(msg + strlen(msg), "n_loaded is "); + msg[strlen(msg)] = '0' + n_loaded; + // ---------- end -------------- break; case -1: strcpy(msg + len, "x is released, eject_2 is "); @@ -85,33 +120,37 @@ 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); + pin3 = 1; + strcpy(msg + len, "triangle is pressed, pin3 is "); + msg[strlen(msg)] = '0' + int(pin3); break; case -3: - back = 0; - strcpy(msg + len, "triangle is released, back is "); - msg[strlen(msg)] = '0' + int(back); + pin3 = 0; + strcpy(msg + len, "triangle is released, pin3 is "); + msg[strlen(msg)] = '0' + int(pin3); break; case 4: strcpy(msg + len, "square is pressed"); - up = 1; down = 1; lock = 1; + pin1 = 1; pin2 = 1; pin3 = 0; lock = 1; break; case -4: strcpy(msg + len, "square is released"); - up = 0; down = 0; + pin1 = 0; pin2 = 0; break; case 5: - down = 1; - up = 0; - strcpy(msg + len, "down arrow is pressed, down is "); - msg[strlen(msg)] = '0' + int(down); + + pin1 = 0; + pin3 = 0; + if(smooth) + pin4 = 1; + pin2 = 1; + strcpy(msg + len, "down arrow is pressed, pin2 is "); + msg[strlen(msg)] = '0' + int(pin2); break; case -5: - down = 0; - strcpy(msg + len, "down arrow is released, down is "); - msg[strlen(msg)] = '0' + int(down); + pin2 = 0; pin4 = 0; + strcpy(msg + len, "down arrow is released, pin2 is "); + msg[strlen(msg)] = '0' + int(pin2); break; case 6: load_manual = 0; @@ -123,8 +162,8 @@ if(n_loaded <= 5) { - dc_motor1.set_out(0,0.7); - wait(1.4); + dc_motor1.set_out(0,0.5); + wait(1.7); dc_motor1.set_out(0,0); } else @@ -132,8 +171,7 @@ load_manual = 1; strcpy(msg + len, "right arrow is pressed. "); strcpy(msg + strlen(msg), "n_loaded cleared!"); - n_loaded = 0; - + n_loaded = 0; } break; @@ -142,15 +180,18 @@ 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); + + pin2 = 0; + if(smooth) + pin4 = 1; + pin1 = 1; + strcpy(msg + len, "up arrow is pressed, pin1 is"); + msg[strlen(msg)] = '0' + int(pin1); break; case -7: - up = 0; - strcpy(msg + len, "up arrow is released, up is"); - msg[strlen(msg)] = '0' + int(up); + pin1 = 0; + strcpy(msg + len, "up arrow is released, pin1 is"); + msg[strlen(msg)] = '0' + int(pin1); break; case 8: strcpy(msg + len, "left arrow is pressed, left is "); @@ -182,7 +223,18 @@ strcpy(msg + len, "left arrow is released, left is "); break; case 9: - strcpy(msg + len, "L1 is pressed"); + auto_shooting = 1 - auto_shooting; + if(auto_shooting) + { + pin4 = 1; pin3 = 1; pin1 = 0; pin2 = 0; + } + else + { + pin3 = 0; pin4 = 0; // dun change order, in case delay + } + + strcpy(msg + len, "L1 is pressed, auto_shooting is "); + msg[strlen(msg)] = '0' + auto_shooting; break; case -9: strcpy(msg + len, "L1 is released"); @@ -196,7 +248,10 @@ strcpy(msg + len, "R1 is released"); break; case 11: - strcpy(msg + len, "share is pressed"); + smooth = 1 - smooth; + strcpy(msg + len, "share is pressed, smooth: "); + msg[strlen(msg)] = '0' + smooth; + break; case -11: strcpy(msg + len, "share is released"); @@ -208,11 +263,11 @@ strcpy(msg + len, "option is released"); break; case 13: - up = 1; down = 1; back = 1; + pin1 = 1; pin2 = 1; pin3 = 1; strcpy(msg + len, "playstation button is pressed, cur pos should be saved"); break; case -13: - up = 0; down = 0; back= 0; + pin1 = 0; pin2 = 0; pin3= 0; strcpy(msg + len, "playstation button is released"); break; default: @@ -234,28 +289,33 @@ nh.initNode(); // Advertise publishers nh.advertise(responser); + nh.advertise(responser2); // Initiate display +// msg = "------- LAST DAY -------"; str_msg.data = msg; + str_msg2.data = msg2; // responser.publish(&str_msg); +// responser2.publish(&str_msg2); // Initiate subscribers nh.subscribe(sub_button); nh.subscribe(sub_joystick); + nh.subscribe(sub_cam); // Initiate IOs, keep everything at rest myled = 0; - back = 0;up = 0; down = 0; lock = 0; + pin1 = 0;pin2 = 0; pin3 = 0; lock = 0; eject_1 = 0; eject_2 = 1; // Initiate firing pins - firing1.mode(PullUp); // default 1 + firing1.mode(PullUp); // default 0 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_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; +// up = 1; pin2 = 1; pin2 = 0; // Main programming int start = 0; // control variable for resetting shooter int tmp = 0; @@ -273,7 +333,7 @@ if(!faul_reset && firing1 == 0) // if(1) { - down = 0; up = 1; back = 1; // 101 + pin2 = 0; pin1 = 1; pin2 = 1; // 101 start = 1; memset(msg, 0, sizeof(msg)); strcpy(msg, "Start resetting Faulhaber: firing1 is "); @@ -307,13 +367,13 @@ } if(firing1.read() == 1 && start == 1) { - up = 0; down = 1; back = 1; // 110 + pin1 = 0; pin2 = 1; pin3 = 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; + pin3 = 0; pin2 = 0; pin1 = 0; start = 0; }