Diff: main.cpp

Revision:

0:ccdc2d6fa225

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jun 29 15:57:10 2020 +0000
@@ -0,0 +1,178 @@
+/*
+ * rosserial Publisher Example
+ * Prints "hello world!"
+ */
+ 
+#define START_FRAME         0xABCD      // [-] Start frme definition for reliable serial communication
+
+#include "mbed.h"
+#include <ros.h>
+#include <std_msgs/String.h>
+#include <geometry_msgs/Twist.h>
+#include "SBUS.h"
+#include "hoverserial.h"
+#include "millis.h"
+
+const int mainLoopInterval = 10;
+unsigned long timeNow = 0;
+unsigned long timeSendNext = 0;
+
+/************************************************
+ * Taranis X9D+ channel name definitions
+ ***********************************************/
+ 
+const uint8_t Thr = 0;
+const uint8_t Rud = 1;
+const uint8_t Ele = 2;
+const uint8_t Ail = 3;
+const uint8_t S1 = 4;
+const uint8_t S2 = 5;
+const uint8_t LS = 6;
+const uint8_t RS = 7;
+const uint8_t SA = 8;
+const uint8_t SB = 9;
+const uint8_t SC = 10;
+const uint8_t SD = 11;
+const uint8_t SE = 12;
+const uint8_t SF = 13;
+const uint8_t SG = 14;
+const uint8_t SH = 15;
+
+/************************************************
+ * Taranis X9D+ channel name definitions
+ ***********************************************/
+
+//ROS
+
+ros::NodeHandle  nh;
+//std_msgs::String str_msg;
+//geometry_msgs::Twist geometry_msg;
+//ros::Publisher chatter("chatter", &str_msg);
+
+//ROS
+
+//Start serial coms for sbus, drivers and pc debug
+
+SBUS::SBUS X8R(PD_5, PD_6);  //SBUS ON UART 2
+//Serial pc(USBTX, USBRX); // tx, rx
+HoverSerial::HoverSerial driverfront(PC_12, PD_2);  // Front driver on UART 5
+HoverSerial::HoverSerial driverrear(PG_14, PG_9);   //Rear driver on uart 6
+
+//End serial start
+
+DigitalOut led = LED1;
+
+typedef struct {                        //Structure for sending data to hoverboard
+    uint16_t start;
+    int16_t  steer;
+    int16_t  speed;
+    uint16_t checksum;
+} SerialCommand;
+SerialCommand Command;
+
+//int val;
+float val;
+
+int controlmode = 0;               //Define control mode (-1 manual, 0 OFF, 1 autonomous (receive from ROS))
+int cmdspeed = 0;
+int cmdsteer = 0;
+int coefspeed = 0;
+int coefsteer = 0;
+int maxspeed = 600;
+int maxsteer = 600;
+int rosspeed = 0;
+int rossteer = 0;
+int reverse;
+int failsafetime;
+int rosmaxvelocity = 30;
+
+void messageCb(const geometry_msgs::Twist& msg){
+    rosspeed = msg.linear.x;
+    rossteer = msg.angular.z;
+}
+
+
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
+
+void testAllChannels() {
+    for(int i = 0; i < 16; i++) {
+        val = X8R.getStickValue(i);
+        //pc.printf("%.2f  ", val);
+        if(i == 15){
+            //pc.printf("\n");
+        }
+    }
+}
+
+int main() {
+    
+    nh.initNode();
+    nh.subscribe(sub);
+    
+    maxspeed = maxspeed/4;
+    maxsteer = maxsteer/4;
+
+    while (1) {
+        controlmode = X8R.getStickValue(SD);
+        coefspeed = X8R.getStickValue(S1) * maxspeed + maxspeed;
+        coefsteer = X8R.getStickValue(S2) * maxsteer + maxsteer;
+        
+        switch(controlmode) {
+            case -1:  //MANUAL MODE
+                //pc.printf("RC MODE \n");
+                if(X8R.checkFailsafeTimer() == 1){     //Checks if data was received in last 12ms (data is sent from X8R receiver every 9ms)
+                    cmdspeed = X8R.getStickValue(Thr) * coefspeed + coefspeed;
+                    cmdsteer = X8R.getStickValue(Ail) * coefsteer;
+                    reverse = X8R.getStickValue(SH);
+                    if(reverse == 1){
+                        cmdspeed = -cmdspeed;
+                    }
+                }
+                else{  //If there was no data (disconnected cable, no receiver power, ...) stop the vehicle and print error on usb/print error in ros
+                    cmdspeed = 0;
+                    cmdsteer = 0;
+                    //pc.printf("RECEIVE ERROR \n");
+                }
+                break;
+                
+            case 0: //STOPPED
+                //pc.printf("STOPPED MODE \n");
+                cmdspeed = 0;
+                cmdsteer = 0;
+                break;
+                
+            case 1: //ROS commands
+                //pc.printf("ROS MODE \n");
+                cmdspeed = rosspeed;
+                cmdsteer = -rossteer;
+                if(cmdspeed > rosmaxvelocity){
+                    cmdspeed = rosmaxvelocity;
+                }
+                if(cmdspeed < -rosmaxvelocity){
+                    cmdspeed = -rosmaxvelocity;
+                }
+                if(cmdsteer > (rosmaxvelocity*2)){
+                    cmdsteer = (rosmaxvelocity*2);
+                }
+                if(cmdsteer < -(rosmaxvelocity*2)){
+                    cmdsteer = -(rosmaxvelocity*2);
+                }
+                break;
+                
+            default: //this should never happen (its a 3 pos switch)
+                //pc.printf("CONTROLMODE ERROR \n");
+                cmdspeed = 0;
+                cmdsteer = 0;
+            }
+            
+        failsafetime = X8R.failSafeTimerMs();
+        
+        //pc.printf("%d %d %d \n", failsafetime, cmdspeed, cmdsteer);  //PRINT
+        
+        driverfront.sendData(cmdsteer, cmdspeed);
+        driverrear.sendData(cmdsteer, cmdspeed);
+        
+        nh.spinOnce();
+        wait_ms(mainLoopInterval);
+    }
+}
\ No newline at end of file