MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 6:0b79b90b83a2
- Parent:
- 5:03af248f3f7c
- Child:
- 7:66c6e14a1d97
- Child:
- 8:9abe58bd7c07
diff -r 03af248f3f7c -r 0b79b90b83a2 main.cpp --- a/main.cpp Mon Jun 14 09:24:44 2021 +0000 +++ b/main.cpp Wed Jun 16 10:10:57 2021 +0000 @@ -23,6 +23,7 @@ // 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("PotCoordinate", msgCam); ros::Subscriber<std_msgs::Int16> sub_cam("PotDistance", msgCam); // Pubs std_msgs::String str_msg; @@ -68,20 +69,44 @@ // 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) + float minimum = 250; + float maximum = 600; + /* + if( cam_msg.data > 0) + { + aout = (maximum - float(cam_msg.data)) / (maximum - minimum); + if(auto_shooting) + { + 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); + } + }*/ + if( cam_msg.data > 0 && auto_shooting) { - 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); + int outValue[3] = [cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10]; + if(auto_shooting) + { + 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); + for(int i = 0; i<3; i++){ + outValue[i]/8; + (outValue[i]/4)%2; + (outValue[i]/2 + (outValue[i]%2; + } + } } + } // button_msg: Int8 @@ -141,8 +166,6 @@ 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); @@ -175,34 +198,34 @@ } break; - case -6: - + case -6: strcpy(msg + len, "right arrow is released. "); break; - case 7: - + case 7: 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: - pin1 = 0; + 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 "); + strcpy(msg + len, "left arrow is pressed, phase 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); + dc_motor2.set_out(0, 1); +// if(firing3 == 1) +// { +// dc_motor2.set_out(0,0); +// strcpy(msg + strlen(msg), ", firing pin detected. "); +// } stage_pick++; eject_1 = 1; eject_2 = 0; + strcpy(msg + strlen(msg), "rotate out"); } else if(stage_pick == 1) { @@ -210,6 +233,7 @@ wait(0.3); dc_motor2.set_out(0,0); stage_pick++; + strcpy(msg + strlen(msg), "rotate phase 1"); } else if(stage_pick == 2) { @@ -217,41 +241,37 @@ wait(0.3); dc_motor2.set_out(0,0); stage_pick = 0; + strcpy(msg + strlen(msg), "rotate phase 2"); } break; case -8: - strcpy(msg + len, "left arrow is released, left is "); +// strcpy(msg + len, "left arrow is released, left is "); break; case 9: - 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; + strcpy(msg + len, "L1 is pressed"); +// msg[strlen(msg)] = '0' + auto_shooting; break; case -9: +// pin3 = 0; pin4 = 0; 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"); + auto_shooting = 1; + pin4 = 1; + strcpy(msg + len, "R1 is pressed, auto_shooting is "); + msg[strlen(msg)] = '0' + auto_shooting; break; case -10: + wait(0.1); + auto_shooting = 0; + pin4 = 0; strcpy(msg + len, "R1 is released"); break; case 11: - smooth = 1 - smooth; - strcpy(msg + len, "share is pressed, smooth: "); - msg[strlen(msg)] = '0' + smooth; - + strcpy(msg + len, "share is pressed"); break; case -11: strcpy(msg + len, "share is released"); @@ -307,8 +327,8 @@ // Initiate firing pins firing1.mode(PullUp); // default 0 - firing2.mode(PullUp); // default 1 - firing3.mode(PullUp); // default 1 + firing2.mode(PullUp); // default 0 + firing3.mode(PullUp); // default 0 // Reset dc motor dc_motor1.reset(); @@ -316,6 +336,8 @@ dc_motor2.reset(); dc_motor2.set_pid(0.008, 0, 0, 0.0); // up = 1; pin2 = 1; pin2 = 0; +// aout.period_us(0.01); + aout = 0; // Main programming int start = 0; // control variable for resetting shooter int tmp = 0; @@ -331,9 +353,8 @@ { myled = 0; if(!faul_reset && firing1 == 0) -// if(1) { - pin2 = 0; pin1 = 1; pin2 = 1; // 101 + pin2 = 0; pin1 = 1; pin3 = 1; // 101 start = 1; memset(msg, 0, sizeof(msg)); strcpy(msg, "Start resetting Faulhaber: firing1 is ");