![](/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
- Committer:
- ftppr
- Date:
- 2021-06-18
- Revision:
- 8:9abe58bd7c07
- Parent:
- 6:0b79b90b83a2
File content as of revision 8:9abe58bd7c07:
// 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/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); // 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/Int16.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 msgCam(const std_msgs::Int16 &cam_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); //ros::Subscriber<std_msgs::Int16> sub_cam("PotCoordinate", msgCam); 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 ============= // External Button DigitalIn button(USER_BUTTON); // LED DigitalOut myled(LED1); // Faulhaber1 pins DigitalOut pin1 = A3; // pin 1 of faulhaber1 DigitalOut pin2 = A5; // pin 2 of faulhaber1 DigitalOut pin3 = A4; // pin 3 of faulhaber1 DigitalOut pin4 = A0; // pin 4 of faulhaber1 AnalogOut aout = D13; // Faulhaber2 pins DigitalOut faul2_1 = A1; // pin1 of faulhaber2 DigitalOut faul2_2 = A2; // pin2 of faulhaber2 // Electromagnet DigitalOut lock = D14; // Firing pin DigitalIn firing1 = D2; // Reset Faulhaber DigitalIn firing2 = D15; // Load 1 arrow DigitalIn firing3 = D7; // Pick, short DigitalIn firing4 = D4; // Pick, longError: Identifier "faul2_1" is undefined in "main.cpp", Line: 241, Col: 14 DigitalIn firing5 = D8; // Pick, long - 2 DigitalIn firing6 = D9; // Valves DigitalOut valve = D3; // valve // DC motors 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, D12, 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; int auto_shooting_goToTargetPos = 0; int start = 0; // control variable for resetting shooter int touch = 0; // cam callback void msgCam(const std_msgs::Int16 &cam_msg) { if( cam_msg.data > 0 && auto_shooting) { int outValue[3] = {cam_msg.data/100, cam_msg.data/10%10, cam_msg.data%10}; if(auto_shooting && auto_shooting_goToTargetPos) { pin1 = 0; pin2 = 0; pin3 = 1; wait(0.2); aout = 1; pin3 = 1; pin2 = 1; pin1 = 1; wait(0.1); auto_shooting_goToTargetPos = false; for(int i = 0; i<3; i++){ //int val[4] = {outValue[i]/8, outValue[i]/4%2, outValue[i]/2%2, outValue[i]%2}; aout = outValue[i]/8; pin3 = outValue[i]/4%2; pin2= outValue[i]/2%2; pin1 = outValue[i]%2; wait(0.1); aout = 1; pin3 = 1; pin2 = 1; pin1 = 1; wait(0.1); } aout = 0; pin3 = 0; pin2 = 0; pin1 = 0; pin4 = 0; } memset(msg2, 0, sizeof(msg2)); strcpy(msg2, "Cam distance: "); itoa(cam_msg.data, msg2 + strlen(msg2), 10); responser2.publish(&str_msg2); } } // 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: valve = 1 - valve; strcpy(msg + len, "x is pressed, valve is "); msg[strlen(msg)] = '0' + int(valve); 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: pin3 = 1; auto_shooting_goToTargetPos = true; strcpy(msg + len, "triangle is pressed, pin3 is "); msg[strlen(msg)] = '0' + int(pin3); break; case -3: 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"); pin1 = 1; pin2 = 1; pin3 = 0; lock = 1; break; case -4: strcpy(msg + len, "square is released"); pin1 = 0; pin2 = 0; break; case 5: pin1 = 0; pin3 = 0; pin2 = 1; strcpy(msg + len, "down arrow is pressed, pin2 is "); msg[strlen(msg)] = '0' + int(pin2); break; case -5: pin2 = 0; pin4 = 0; strcpy(msg + len, "down arrow is released, pin2 is "); msg[strlen(msg)] = '0' + int(pin2); break; case 6: strcpy(msg + len, "right arrow is pressed. "); msg[strlen(msg)] = '0' + n_loaded; dc_motor1.set_out(0,1); n_loaded++; while(firing2 == 0) { wait(0.05); } if(n_loaded <= 5) { dc_motor1.set_out(0,0.7); wait(1.4); dc_motor1.set_out(0,0); } else { wait(0.21); dc_motor1.set_out(0,0); strcpy(msg + strlen(msg), "n_loaded cleared!"); n_loaded = 0; } break; case -6: strcpy(msg + len, "right arrow is released. "); break; case 7: pin2 = 0; pin1 = 1; strcpy(msg + len, "up arrow is pressed, pin1 is"); msg[strlen(msg)] = '0' + int(pin1); break; case -7: 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, phase is "); msg[strlen(msg)] = '0' + stage_pick; strcpy(msg + strlen(msg), " "); if(stage_pick == 0) { strcpy(msg + strlen(msg), "dc_motor reset "); dc_motor2.set_out(0.5, 0); while(firing3 == 0); if(firing3 == 1) { dc_motor2.set_out(0,0); strcpy(msg + strlen(msg), "short: firing3 pin detected "); } stage_pick++; // eject_1 = 1; eject_2 = 0; } else if(stage_pick == 1) { strcpy(msg + strlen(msg), "faulhaber 2 RefPos "); faul2_1 = 1; while(firing4 == 0); faul2_1 = 0; strcpy(msg + strlen(msg), "long: firing4 detected "); //內 stage_pick++; } else if(stage_pick == 2) { strcpy(msg + strlen(msg), "dc_motor rotate to pick up "); dc_motor2.set_out(0,0.5); wait(0.2); dc_motor2.set_out(0,0); stage_pick++; // stage_pick = 0; } else if(stage_pick == 3) { strcpy(msg + strlen(msg), "faul2 to pick the arrows "); // 外 faul2_2 = 1; while(firing5 == 0); wait(0.3); faul2_2 = 0; strcpy(msg + strlen(msg), "Firing5 touched "); stage_pick++; // use firing 5 // stage_pick = 0; } else if(stage_pick == 4) { strcpy(msg + strlen(msg), "faul2 rotate to vertical \n\r"); // 內一啲 faul2_1 = 1; wait(2.6); faul2_1 = 0; stage_pick++; // stage_pick = 0; } else if(stage_pick == 5) { strcpy(msg + strlen(msg), "dc_motor rotate to final pos\n\r"); dc_motor2.set_out(0,1); // wait(2); // when there is load; otherwise 0.3 while(firing6 == 0); // { // wait(0.025); // } dc_motor2.set_out(0,0); stage_pick++; // stage_pick = 0; } else if(stage_pick == 6) { strcpy(msg + strlen(msg), "faul2 rotate to final pos \n\r"); // 內一啲 faul2_1 = 1; wait(1.9); faul2_1 = 0; stage_pick = 0; } break; case -8: break; case 9: strcpy(msg + len, "L1 is pressed"); 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); auto_shooting = 1; pin4 = 1; n_loaded = 6; 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: strcpy(msg + len, "share is pressed"); strcpy(msg + strlen(msg), "Start resetting Faulhaber: firing1 is "); msg[strlen(msg)] = '0' + firing1.read(); responser.publish(&str_msg); if(!faul_reset && firing1 == 0) { pin2 = 0; pin1 = 1; pin3 = 1; // 101 memset(msg, 0, sizeof(msg)); // faul_reset = 1; } while(firing1 == 0); pin1 = 0; pin2 = 1; pin3 = 1; // 110 wait(0.3); pin1 = 0; pin2 = 0; pin3 = 0; // memset(msg, 0, sizeof(msg)); strcpy(msg + strlen(msg), " case 11 firing1 is touched"); 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: pin1 = 1; pin2 = 1; pin3 = 1; strcpy(msg + len, "playstation button is pressed, cur pos should be saved"); break; case -13: pin1 = 0; pin2 = 0; pin3= 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); 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; pin1 = 0;pin2 = 0; pin3 = 0; lock = 0; valve = 0; faul2_1 = 0; faul2_2 = 0; // Initiate firing pins firing1.mode(PullUp); // default 0 firing2.mode(PullUp); // default 0 firing3.mode(PullUp); // default 0 firing4.mode(PullUp); // default 0 firing5.mode(PullUp); // default 0 firing6.mode(PullUp); // default 0 // 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; pin2 = 1; pin2 = 0; // aout.period_us(0.01); aout = 0; // 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 } } 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, "R2: the value is "); len = strlen(msg); itoa(joystick_msg.data[2], msg + len, 10); } responser.publish(&str_msg); }