MBed program for TR1 functions
Dependencies: mbed Servo ros_lib_melodic DC_Stepper_Controller_Lib
Diff: main.cpp
- Revision:
- 0:1bf7e0755f40
- Child:
- 1:fbdbf4ac12c6
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Wed Jun 02 01:14:08 2021 +0000 @@ -0,0 +1,327 @@ + // Hello World to sweep a servo through its full range +// 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> +ros::NodeHandle nh; +DigitalOut myled(LED1); +void msgCb_joystick(const std_msgs::Int16MultiArray &joystick_msg); +void msgCb_button(const std_msgs::Int8 &button_msg); + +ros::Subscriber<std_msgs::Int16MultiArray> sub_joystick("joystick", msgCb_joystick); +ros::Subscriber<std_msgs::Int8> sub_button("button", msgCb_button); + +std_msgs::String str_msg; +ros::Publisher responser("display", &str_msg); + +char msg[200] = {0}; + +DigitalIn button(USER_BUTTON); + +DigitalOut up = D12; // 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 = D15; // 電磁石 +DigitalOut back = A4; // pin 3 of faulhaber +DigitalOut eject_1 = D10; // valve 1 +DigitalOut eject_2 = D3; // valve 2 +//DigitalOut savePos = D0; savepos by back=1 & up=1 & down=1 +DigitalIn firing = D3; + + +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); +} + +// 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 = 1; up = 0; + 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