Luka Slapnik
final
Dependencies: mbed hoverserial ros_lib_kinetic sbus_decode1 millis
Revision 0:ccdc2d6fa225, committed 2020-06-29
- Comitter:
- sllez
- Date:
- Mon Jun 29 15:57:10 2020 +0000
- Commit message:
- final
Changed in this revision
diff -r 000000000000 -r ccdc2d6fa225 hoverserial.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/hoverserial.lib Mon Jun 29 15:57:10 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sllez/code/hoverserial/#ca040e149431
diff -r 000000000000 -r ccdc2d6fa225 main.cpp
--- /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
diff -r 000000000000 -r ccdc2d6fa225 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jun 29 15:57:10 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9baf128c2fab \ No newline at end of file
diff -r 000000000000 -r ccdc2d6fa225 millis.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/millis.lib Mon Jun 29 15:57:10 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/hudakz/code/millis/#ac7586424119
diff -r 000000000000 -r ccdc2d6fa225 ros_lib_kinetic.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ros_lib_kinetic.lib Mon Jun 29 15:57:10 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/garyservin/code/ros_lib_kinetic/#a849bf78d77f
diff -r 000000000000 -r ccdc2d6fa225 sbus_decode.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sbus_decode.lib Mon Jun 29 15:57:10 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/sllez/code/sbus_decode1/#c96df23cad7d