Diff: main.cpp

Revision:

0:1bf7e0755f40

Child:

1:fbdbf4ac12c6

diff -r 000000000000 -r 1bf7e0755f40 main.cpp
--- /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