Luka Slapnik / Mbed 2 deprecated CyberbotV1

final

Dependencies:   mbed hoverserial ros_lib_kinetic sbus_decode1 millis

main.cpp@0:ccdc2d6fa225, 2020-06-29 (annotated)

Committer:
sllez
Date:
Mon Jun 29 15:57:10 2020 +0000
Revision:
0:ccdc2d6fa225
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sllez 0:ccdc2d6fa225 1 /*
sllez 0:ccdc2d6fa225 2 * rosserial Publisher Example
sllez 0:ccdc2d6fa225 3 * Prints "hello world!"
sllez 0:ccdc2d6fa225 4 */
sllez 0:ccdc2d6fa225 5
sllez 0:ccdc2d6fa225 6 #define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
sllez 0:ccdc2d6fa225 7
sllez 0:ccdc2d6fa225 8 #include "mbed.h"
sllez 0:ccdc2d6fa225 9 #include <ros.h>
sllez 0:ccdc2d6fa225 10 #include <std_msgs/String.h>
sllez 0:ccdc2d6fa225 11 #include <geometry_msgs/Twist.h>
sllez 0:ccdc2d6fa225 12 #include "SBUS.h"
sllez 0:ccdc2d6fa225 13 #include "hoverserial.h"
sllez 0:ccdc2d6fa225 14 #include "millis.h"
sllez 0:ccdc2d6fa225 15
sllez 0:ccdc2d6fa225 16 const int mainLoopInterval = 10;
sllez 0:ccdc2d6fa225 17 unsigned long timeNow = 0;
sllez 0:ccdc2d6fa225 18 unsigned long timeSendNext = 0;
sllez 0:ccdc2d6fa225 19
sllez 0:ccdc2d6fa225 20 /************************************************
sllez 0:ccdc2d6fa225 21 * Taranis X9D+ channel name definitions
sllez 0:ccdc2d6fa225 22 ***********************************************/
sllez 0:ccdc2d6fa225 23
sllez 0:ccdc2d6fa225 24 const uint8_t Thr = 0;
sllez 0:ccdc2d6fa225 25 const uint8_t Rud = 1;
sllez 0:ccdc2d6fa225 26 const uint8_t Ele = 2;
sllez 0:ccdc2d6fa225 27 const uint8_t Ail = 3;
sllez 0:ccdc2d6fa225 28 const uint8_t S1 = 4;
sllez 0:ccdc2d6fa225 29 const uint8_t S2 = 5;
sllez 0:ccdc2d6fa225 30 const uint8_t LS = 6;
sllez 0:ccdc2d6fa225 31 const uint8_t RS = 7;
sllez 0:ccdc2d6fa225 32 const uint8_t SA = 8;
sllez 0:ccdc2d6fa225 33 const uint8_t SB = 9;
sllez 0:ccdc2d6fa225 34 const uint8_t SC = 10;
sllez 0:ccdc2d6fa225 35 const uint8_t SD = 11;
sllez 0:ccdc2d6fa225 36 const uint8_t SE = 12;
sllez 0:ccdc2d6fa225 37 const uint8_t SF = 13;
sllez 0:ccdc2d6fa225 38 const uint8_t SG = 14;
sllez 0:ccdc2d6fa225 39 const uint8_t SH = 15;
sllez 0:ccdc2d6fa225 40
sllez 0:ccdc2d6fa225 41 /************************************************
sllez 0:ccdc2d6fa225 42 * Taranis X9D+ channel name definitions
sllez 0:ccdc2d6fa225 43 ***********************************************/
sllez 0:ccdc2d6fa225 44
sllez 0:ccdc2d6fa225 45 //ROS
sllez 0:ccdc2d6fa225 46
sllez 0:ccdc2d6fa225 47 ros::NodeHandle nh;
sllez 0:ccdc2d6fa225 48 //std_msgs::String str_msg;
sllez 0:ccdc2d6fa225 49 //geometry_msgs::Twist geometry_msg;
sllez 0:ccdc2d6fa225 50 //ros::Publisher chatter("chatter", &str_msg);
sllez 0:ccdc2d6fa225 51
sllez 0:ccdc2d6fa225 52 //ROS
sllez 0:ccdc2d6fa225 53
sllez 0:ccdc2d6fa225 54 //Start serial coms for sbus, drivers and pc debug
sllez 0:ccdc2d6fa225 55
sllez 0:ccdc2d6fa225 56 SBUS::SBUS X8R(PD_5, PD_6); //SBUS ON UART 2
sllez 0:ccdc2d6fa225 57 //Serial pc(USBTX, USBRX); // tx, rx
sllez 0:ccdc2d6fa225 58 HoverSerial::HoverSerial driverfront(PC_12, PD_2); // Front driver on UART 5
sllez 0:ccdc2d6fa225 59 HoverSerial::HoverSerial driverrear(PG_14, PG_9); //Rear driver on uart 6
sllez 0:ccdc2d6fa225 60
sllez 0:ccdc2d6fa225 61 //End serial start
sllez 0:ccdc2d6fa225 62
sllez 0:ccdc2d6fa225 63 DigitalOut led = LED1;
sllez 0:ccdc2d6fa225 64
sllez 0:ccdc2d6fa225 65 typedef struct { //Structure for sending data to hoverboard
sllez 0:ccdc2d6fa225 66 uint16_t start;
sllez 0:ccdc2d6fa225 67 int16_t steer;
sllez 0:ccdc2d6fa225 68 int16_t speed;
sllez 0:ccdc2d6fa225 69 uint16_t checksum;
sllez 0:ccdc2d6fa225 70 } SerialCommand;
sllez 0:ccdc2d6fa225 71 SerialCommand Command;
sllez 0:ccdc2d6fa225 72
sllez 0:ccdc2d6fa225 73 //int val;
sllez 0:ccdc2d6fa225 74 float val;
sllez 0:ccdc2d6fa225 75
sllez 0:ccdc2d6fa225 76 int controlmode = 0; //Define control mode (-1 manual, 0 OFF, 1 autonomous (receive from ROS))
sllez 0:ccdc2d6fa225 77 int cmdspeed = 0;
sllez 0:ccdc2d6fa225 78 int cmdsteer = 0;
sllez 0:ccdc2d6fa225 79 int coefspeed = 0;
sllez 0:ccdc2d6fa225 80 int coefsteer = 0;
sllez 0:ccdc2d6fa225 81 int maxspeed = 600;
sllez 0:ccdc2d6fa225 82 int maxsteer = 600;
sllez 0:ccdc2d6fa225 83 int rosspeed = 0;
sllez 0:ccdc2d6fa225 84 int rossteer = 0;
sllez 0:ccdc2d6fa225 85 int reverse;
sllez 0:ccdc2d6fa225 86 int failsafetime;
sllez 0:ccdc2d6fa225 87 int rosmaxvelocity = 30;
sllez 0:ccdc2d6fa225 88
sllez 0:ccdc2d6fa225 89 void messageCb(const geometry_msgs::Twist& msg){
sllez 0:ccdc2d6fa225 90 rosspeed = msg.linear.x;
sllez 0:ccdc2d6fa225 91 rossteer = msg.angular.z;
sllez 0:ccdc2d6fa225 92 }
sllez 0:ccdc2d6fa225 93
sllez 0:ccdc2d6fa225 94
sllez 0:ccdc2d6fa225 95 ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb);
sllez 0:ccdc2d6fa225 96
sllez 0:ccdc2d6fa225 97 void testAllChannels() {
sllez 0:ccdc2d6fa225 98 for(int i = 0; i < 16; i++) {
sllez 0:ccdc2d6fa225 99 val = X8R.getStickValue(i);
sllez 0:ccdc2d6fa225 100 //pc.printf("%.2f ", val);
sllez 0:ccdc2d6fa225 101 if(i == 15){
sllez 0:ccdc2d6fa225 102 //pc.printf("\n");
sllez 0:ccdc2d6fa225 103 }
sllez 0:ccdc2d6fa225 104 }
sllez 0:ccdc2d6fa225 105 }
sllez 0:ccdc2d6fa225 106
sllez 0:ccdc2d6fa225 107 int main() {
sllez 0:ccdc2d6fa225 108
sllez 0:ccdc2d6fa225 109 nh.initNode();
sllez 0:ccdc2d6fa225 110 nh.subscribe(sub);
sllez 0:ccdc2d6fa225 111
sllez 0:ccdc2d6fa225 112 maxspeed = maxspeed/4;
sllez 0:ccdc2d6fa225 113 maxsteer = maxsteer/4;
sllez 0:ccdc2d6fa225 114
sllez 0:ccdc2d6fa225 115 while (1) {
sllez 0:ccdc2d6fa225 116 controlmode = X8R.getStickValue(SD);
sllez 0:ccdc2d6fa225 117 coefspeed = X8R.getStickValue(S1) * maxspeed + maxspeed;
sllez 0:ccdc2d6fa225 118 coefsteer = X8R.getStickValue(S2) * maxsteer + maxsteer;
sllez 0:ccdc2d6fa225 119
sllez 0:ccdc2d6fa225 120 switch(controlmode) {
sllez 0:ccdc2d6fa225 121 case -1: //MANUAL MODE
sllez 0:ccdc2d6fa225 122 //pc.printf("RC MODE \n");
sllez 0:ccdc2d6fa225 123 if(X8R.checkFailsafeTimer() == 1){ //Checks if data was received in last 12ms (data is sent from X8R receiver every 9ms)
sllez 0:ccdc2d6fa225 124 cmdspeed = X8R.getStickValue(Thr) * coefspeed + coefspeed;
sllez 0:ccdc2d6fa225 125 cmdsteer = X8R.getStickValue(Ail) * coefsteer;
sllez 0:ccdc2d6fa225 126 reverse = X8R.getStickValue(SH);
sllez 0:ccdc2d6fa225 127 if(reverse == 1){
sllez 0:ccdc2d6fa225 128 cmdspeed = -cmdspeed;
sllez 0:ccdc2d6fa225 129 }
sllez 0:ccdc2d6fa225 130 }
sllez 0:ccdc2d6fa225 131 else{ //If there was no data (disconnected cable, no receiver power, ...) stop the vehicle and print error on usb/print error in ros
sllez 0:ccdc2d6fa225 132 cmdspeed = 0;
sllez 0:ccdc2d6fa225 133 cmdsteer = 0;
sllez 0:ccdc2d6fa225 134 //pc.printf("RECEIVE ERROR \n");
sllez 0:ccdc2d6fa225 135 }
sllez 0:ccdc2d6fa225 136 break;
sllez 0:ccdc2d6fa225 137
sllez 0:ccdc2d6fa225 138 case 0: //STOPPED
sllez 0:ccdc2d6fa225 139 //pc.printf("STOPPED MODE \n");
sllez 0:ccdc2d6fa225 140 cmdspeed = 0;
sllez 0:ccdc2d6fa225 141 cmdsteer = 0;
sllez 0:ccdc2d6fa225 142 break;
sllez 0:ccdc2d6fa225 143
sllez 0:ccdc2d6fa225 144 case 1: //ROS commands
sllez 0:ccdc2d6fa225 145 //pc.printf("ROS MODE \n");
sllez 0:ccdc2d6fa225 146 cmdspeed = rosspeed;
sllez 0:ccdc2d6fa225 147 cmdsteer = -rossteer;
sllez 0:ccdc2d6fa225 148 if(cmdspeed > rosmaxvelocity){
sllez 0:ccdc2d6fa225 149 cmdspeed = rosmaxvelocity;
sllez 0:ccdc2d6fa225 150 }
sllez 0:ccdc2d6fa225 151 if(cmdspeed < -rosmaxvelocity){
sllez 0:ccdc2d6fa225 152 cmdspeed = -rosmaxvelocity;
sllez 0:ccdc2d6fa225 153 }
sllez 0:ccdc2d6fa225 154 if(cmdsteer > (rosmaxvelocity*2)){
sllez 0:ccdc2d6fa225 155 cmdsteer = (rosmaxvelocity*2);
sllez 0:ccdc2d6fa225 156 }
sllez 0:ccdc2d6fa225 157 if(cmdsteer < -(rosmaxvelocity*2)){
sllez 0:ccdc2d6fa225 158 cmdsteer = -(rosmaxvelocity*2);
sllez 0:ccdc2d6fa225 159 }
sllez 0:ccdc2d6fa225 160 break;
sllez 0:ccdc2d6fa225 161
sllez 0:ccdc2d6fa225 162 default: //this should never happen (its a 3 pos switch)
sllez 0:ccdc2d6fa225 163 //pc.printf("CONTROLMODE ERROR \n");
sllez 0:ccdc2d6fa225 164 cmdspeed = 0;
sllez 0:ccdc2d6fa225 165 cmdsteer = 0;
sllez 0:ccdc2d6fa225 166 }
sllez 0:ccdc2d6fa225 167
sllez 0:ccdc2d6fa225 168 failsafetime = X8R.failSafeTimerMs();
sllez 0:ccdc2d6fa225 169
sllez 0:ccdc2d6fa225 170 //pc.printf("%d %d %d \n", failsafetime, cmdspeed, cmdsteer); //PRINT
sllez 0:ccdc2d6fa225 171
sllez 0:ccdc2d6fa225 172 driverfront.sendData(cmdsteer, cmdspeed);
sllez 0:ccdc2d6fa225 173 driverrear.sendData(cmdsteer, cmdspeed);
sllez 0:ccdc2d6fa225 174
sllez 0:ccdc2d6fa225 175 nh.spinOnce();
sllez 0:ccdc2d6fa225 176 wait_ms(mainLoopInterval);
sllez 0:ccdc2d6fa225 177 }
sllez 0:ccdc2d6fa225 178 }