Andrew Wood / Mbed 2 deprecated ThrusterTest2019

Working Thruster and Servo RC

Dependencies:   PwmIn mbed Servo

main.cpp@4:e02ad2d9ba69, 2019-06-16 (annotated)

Committer:
WoodyERAU
Date:
Sun Jun 16 19:35:05 2019 +0000
Revision:
4:e02ad2d9ba69
Parent:
3:64b5ea1e088f 
Thruster and Servo Control with Labview interface.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WoodyERAU 0:4fb155dd1c7a 1 #include "mbed.h"
WoodyERAU 1:1d2b72d09c56 2 #include "Servo.h"
WoodyERAU 1:1d2b72d09c56 3 #include "PwmIn.h"
WoodyERAU 1:1d2b72d09c56 4
WoodyERAU 4:e02ad2d9ba69 5
WoodyERAU 4:e02ad2d9ba69 6
WoodyERAU 4:e02ad2d9ba69 7
WoodyERAU 4:e02ad2d9ba69 8
WoodyERAU 4:e02ad2d9ba69 9
WoodyERAU 4:e02ad2d9ba69 10 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 11 ///////////////////////////// setup //////////////////////////////
WoodyERAU 4:e02ad2d9ba69 12 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 13
WoodyERAU 1:1d2b72d09c56 14 //Initialize left and right servos
WoodyERAU 2:cc7237f357e4 15 Servo back_servo(p23);
WoodyERAU 2:cc7237f357e4 16 Servo front_servo(p24);
WoodyERAU 2:cc7237f357e4 17
WoodyERAU 2:cc7237f357e4 18
WoodyERAU 1:1d2b72d09c56 19 //Pins for Thruster Power
WoodyERAU 2:cc7237f357e4 20 Servo front_thrust(p25);
WoodyERAU 2:cc7237f357e4 21 Servo back_thrust(p26);
WoodyERAU 1:1d2b72d09c56 22
WoodyERAU 1:1d2b72d09c56 23
WoodyERAU 1:1d2b72d09c56 24 //Initialize receiver pins
WoodyERAU 1:1d2b72d09c56 25 PwmIn thro(p17);
WoodyERAU 1:1d2b72d09c56 26 PwmIn elev(p15);
WoodyERAU 1:1d2b72d09c56 27 PwmIn gear(p13);
WoodyERAU 1:1d2b72d09c56 28 PwmIn aile(p14);
WoodyERAU 1:1d2b72d09c56 29 PwmIn rudd(p16);
WoodyERAU 4:e02ad2d9ba69 30 PwmIn aux(p18);
WoodyERAU 4:e02ad2d9ba69 31
WoodyERAU 4:e02ad2d9ba69 32 Serial pc(USBTX, USBRX);
WoodyERAU 4:e02ad2d9ba69 33
WoodyERAU 4:e02ad2d9ba69 34
WoodyERAU 4:e02ad2d9ba69 35 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 36 ///////////////////////////// global variables //////////////////////////////
WoodyERAU 4:e02ad2d9ba69 37 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 1:1d2b72d09c56 38
WoodyERAU 1:1d2b72d09c56 39 //Set all PWM values to a defaalt of 0
WoodyERAU 1:1d2b72d09c56 40 float throttle_output = 0.0;
WoodyERAU 1:1d2b72d09c56 41 float elevation_output = 0.0;
WoodyERAU 1:1d2b72d09c56 42 float ESTOP_output = 0.0;
WoodyERAU 1:1d2b72d09c56 43 float rudder_output = 0.0;
WoodyERAU 1:1d2b72d09c56 44 float aileron_output = 0.0;
WoodyERAU 4:e02ad2d9ba69 45 float aux_output = 0.0;
WoodyERAU 4:e02ad2d9ba69 46
WoodyERAU 4:e02ad2d9ba69 47 //Variables for Serial Communcation with Labview
WoodyERAU 4:e02ad2d9ba69 48 volatile bool newData = false;
WoodyERAU 4:e02ad2d9ba69 49 volatile float inputs[4];
WoodyERAU 4:e02ad2d9ba69 50
WoodyERAU 4:e02ad2d9ba69 51
WoodyERAU 4:e02ad2d9ba69 52
WoodyERAU 4:e02ad2d9ba69 53 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 54 ///////////////////////////// Functions ///////////////////////////////
WoodyERAU 4:e02ad2d9ba69 55 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 56
WoodyERAU 4:e02ad2d9ba69 57 //Function changes 0 t 1 input into 0.08 to 0.92
WoodyERAU 4:e02ad2d9ba69 58 float RangeChange(float x)
WoodyERAU 4:e02ad2d9ba69 59 {
WoodyERAU 4:e02ad2d9ba69 60 float y;
WoodyERAU 4:e02ad2d9ba69 61 y = ((x * 0.84) + 0.08);
WoodyERAU 4:e02ad2d9ba69 62 return y;
WoodyERAU 4:e02ad2d9ba69 63 }
WoodyERAU 4:e02ad2d9ba69 64
WoodyERAU 4:e02ad2d9ba69 65
WoodyERAU 4:e02ad2d9ba69 66
WoodyERAU 4:e02ad2d9ba69 67
WoodyERAU 0:4fb155dd1c7a 68
WoodyERAU 0:4fb155dd1c7a 69
WoodyERAU 4:e02ad2d9ba69 70 //Function Reads Serial com
WoodyERAU 4:e02ad2d9ba69 71 void onSerialRx()
WoodyERAU 4:e02ad2d9ba69 72 {
WoodyERAU 4:e02ad2d9ba69 73 static char serialInBuffer[100];
WoodyERAU 4:e02ad2d9ba69 74 static int serialCount = 0;
WoodyERAU 4:e02ad2d9ba69 75
WoodyERAU 4:e02ad2d9ba69 76 while (pc.readable())// in case two bytes are ready
WoodyERAU 4:e02ad2d9ba69 77 {
WoodyERAU 4:e02ad2d9ba69 78 char byteIn = pc.getc();//Read serial message
WoodyERAU 4:e02ad2d9ba69 79
WoodyERAU 4:e02ad2d9ba69 80 if (byteIn == 0x65)// if an end of line is found
WoodyERAU 4:e02ad2d9ba69 81 {
WoodyERAU 4:e02ad2d9ba69 82 serialInBuffer[serialCount] == 0; // null terminate the input
WoodyERAU 4:e02ad2d9ba69 83 float w,x,y,z;
WoodyERAU 4:e02ad2d9ba69 84 if (sscanf(serialInBuffer,"%f,%f,%f,%fe",&w,&x,&y,&z) == 4)// managed to read all 4 values
WoodyERAU 4:e02ad2d9ba69 85 {
WoodyERAU 4:e02ad2d9ba69 86 inputs[0] = w;
WoodyERAU 4:e02ad2d9ba69 87 inputs[1] = x;
WoodyERAU 4:e02ad2d9ba69 88 inputs[2] = y;
WoodyERAU 4:e02ad2d9ba69 89 inputs[3] = z;
WoodyERAU 4:e02ad2d9ba69 90 newData = true;
WoodyERAU 4:e02ad2d9ba69 91 }
WoodyERAU 4:e02ad2d9ba69 92 serialCount = 0; // reset the buffer
WoodyERAU 4:e02ad2d9ba69 93 }
WoodyERAU 4:e02ad2d9ba69 94 else
WoodyERAU 4:e02ad2d9ba69 95 {
WoodyERAU 4:e02ad2d9ba69 96 serialInBuffer[serialCount] = byteIn;// store the character
WoodyERAU 4:e02ad2d9ba69 97 if (serialCount<100)
WoodyERAU 4:e02ad2d9ba69 98 {
WoodyERAU 4:e02ad2d9ba69 99 serialCount++;// increase the counter.
WoodyERAU 4:e02ad2d9ba69 100 }
WoodyERAU 4:e02ad2d9ba69 101 }
WoodyERAU 4:e02ad2d9ba69 102 }
WoodyERAU 4:e02ad2d9ba69 103 }
WoodyERAU 4:e02ad2d9ba69 104
WoodyERAU 4:e02ad2d9ba69 105
WoodyERAU 4:e02ad2d9ba69 106
WoodyERAU 4:e02ad2d9ba69 107
WoodyERAU 4:e02ad2d9ba69 108 void Calibrate()
WoodyERAU 4:e02ad2d9ba69 109 {
WoodyERAU 1:1d2b72d09c56 110 //Calibration Sequence
WoodyERAU 2:cc7237f357e4 111 back_servo = 0.0;
WoodyERAU 2:cc7237f357e4 112 front_servo = 0.0;
WoodyERAU 2:cc7237f357e4 113 back_thrust = 0.0;
WoodyERAU 2:cc7237f357e4 114 front_thrust = 0.0;
WoodyERAU 4:e02ad2d9ba69 115 wait(0.1); //ESC detects signal
WoodyERAU 1:1d2b72d09c56 116 //Required ESC Calibration/Arming sequence
WoodyERAU 1:1d2b72d09c56 117 //sends longest and shortest PWM pulse to learn and arm at power on
WoodyERAU 2:cc7237f357e4 118 back_servo = 1.0;
WoodyERAU 2:cc7237f357e4 119 front_servo = 1.0;
WoodyERAU 2:cc7237f357e4 120 back_thrust = 1.0;
WoodyERAU 2:cc7237f357e4 121 front_thrust = 1.0;
WoodyERAU 3:64b5ea1e088f 122 wait(0.1);
WoodyERAU 2:cc7237f357e4 123 back_servo = 0.0;
WoodyERAU 2:cc7237f357e4 124 front_servo = 0.0;
WoodyERAU 2:cc7237f357e4 125 back_thrust = 0.0;
WoodyERAU 2:cc7237f357e4 126 front_thrust = 0.0;
WoodyERAU 4:e02ad2d9ba69 127 wait(0.1);
WoodyERAU 1:1d2b72d09c56 128 //End calibration sequence
WoodyERAU 4:e02ad2d9ba69 129 }
WoodyERAU 4:e02ad2d9ba69 130
WoodyERAU 4:e02ad2d9ba69 131
WoodyERAU 4:e02ad2d9ba69 132
WoodyERAU 4:e02ad2d9ba69 133 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 134 ///////////////////////////// Main ///////////////////////////////
WoodyERAU 4:e02ad2d9ba69 135 ////////////////////////////////////////////////////////////////////////////////
WoodyERAU 4:e02ad2d9ba69 136
WoodyERAU 4:e02ad2d9ba69 137 int main() {
WoodyERAU 1:1d2b72d09c56 138
WoodyERAU 4:e02ad2d9ba69 139 Calibrate();
WoodyERAU 4:e02ad2d9ba69 140
WoodyERAU 4:e02ad2d9ba69 141 pc.attach(&onSerialRx);
WoodyERAU 4:e02ad2d9ba69 142 unsigned int expired = 0;
WoodyERAU 2:cc7237f357e4 143
WoodyERAU 1:1d2b72d09c56 144 while(1) {
WoodyERAU 2:cc7237f357e4 145
WoodyERAU 4:e02ad2d9ba69 146
WoodyERAU 2:cc7237f357e4 147 //Enable Servo to turn 180 degrees
WoodyERAU 2:cc7237f357e4 148 back_servo.calibrate(0.00085,90.0);
WoodyERAU 2:cc7237f357e4 149 front_servo.calibrate(0.00085,90.0);
WoodyERAU 2:cc7237f357e4 150
WoodyERAU 1:1d2b72d09c56 151
WoodyERAU 1:1d2b72d09c56 152 //Read in all PWM signals and set them to a value between 0 and 1
WoodyERAU 2:cc7237f357e4 153 elevation_output = (elev.pulsewidth()*1000)-1;
WoodyERAU 2:cc7237f357e4 154 throttle_output = (thro.pulsewidth()*1000)-1;
WoodyERAU 2:cc7237f357e4 155 rudder_output = (rudd.pulsewidth()*1000)-1;
WoodyERAU 2:cc7237f357e4 156 aileron_output = (aile.pulsewidth()*1000)-1;
WoodyERAU 1:1d2b72d09c56 157
WoodyERAU 4:e02ad2d9ba69 158 //RC vs Auto PWM
WoodyERAU 4:e02ad2d9ba69 159 aux_output = (aux.pulsewidth()*1000)-1; // >.5 RC... <.5 Auto
WoodyERAU 4:e02ad2d9ba69 160
WoodyERAU 1:1d2b72d09c56 161 //ESTOP PWM
WoodyERAU 1:1d2b72d09c56 162 ESTOP_output = (gear.pulsewidth()*1000)-1; // >.5 run... <.5 STOP
WoodyERAU 1:1d2b72d09c56 163
WoodyERAU 4:e02ad2d9ba69 164
WoodyERAU 4:e02ad2d9ba69 165
WoodyERAU 1:1d2b72d09c56 166 if(ESTOP_output > 0.5)
WoodyERAU 4:e02ad2d9ba69 167 {
WoodyERAU 2:cc7237f357e4 168
WoodyERAU 4:e02ad2d9ba69 169 if(aux_output > 0.5) //RC
WoodyERAU 4:e02ad2d9ba69 170 {
WoodyERAU 2:cc7237f357e4 171 //Servo Controllers
WoodyERAU 2:cc7237f357e4 172 back_servo = aileron_output;
WoodyERAU 2:cc7237f357e4 173 front_servo = rudder_output;
WoodyERAU 2:cc7237f357e4 174
WoodyERAU 2:cc7237f357e4 175 //Thrust Controllers
WoodyERAU 2:cc7237f357e4 176 back_thrust = throttle_output - 0.04;
WoodyERAU 2:cc7237f357e4 177 front_thrust = elevation_output - 0.04;
WoodyERAU 4:e02ad2d9ba69 178
WoodyERAU 1:1d2b72d09c56 179 }
WoodyERAU 4:e02ad2d9ba69 180 else //Auto
WoodyERAU 4:e02ad2d9ba69 181 {
WoodyERAU 4:e02ad2d9ba69 182 if (newData)
WoodyERAU 4:e02ad2d9ba69 183 {
WoodyERAU 4:e02ad2d9ba69 184 newData = false;//Reset NewData Boolean
WoodyERAU 4:e02ad2d9ba69 185
WoodyERAU 4:e02ad2d9ba69 186 front_servo = inputs[0];//Set thruster values
WoodyERAU 4:e02ad2d9ba69 187 front_thrust = RangeChange(inputs[1]) - 0.04;
WoodyERAU 4:e02ad2d9ba69 188 back_servo = inputs[2];
WoodyERAU 4:e02ad2d9ba69 189 back_thrust = RangeChange(inputs[3]) - 0.04;
WoodyERAU 4:e02ad2d9ba69 190
WoodyERAU 4:e02ad2d9ba69 191 expired = 0;//Reset Expried
WoodyERAU 4:e02ad2d9ba69 192 }
WoodyERAU 4:e02ad2d9ba69 193 else
WoodyERAU 4:e02ad2d9ba69 194 {
WoodyERAU 4:e02ad2d9ba69 195 expired++; //Count the number of loops without new data
WoodyERAU 4:e02ad2d9ba69 196 }
WoodyERAU 2:cc7237f357e4 197
WoodyERAU 4:e02ad2d9ba69 198 if(expired > 10000000) //If too many loops have passed with no new data, assume Labview crashed
WoodyERAU 4:e02ad2d9ba69 199 {
WoodyERAU 4:e02ad2d9ba69 200 //indicate loss of labview com
WoodyERAU 4:e02ad2d9ba69 201 back_thrust = 0.46;
WoodyERAU 4:e02ad2d9ba69 202 front_thrust = 0.46;
WoodyERAU 4:e02ad2d9ba69 203 }
WoodyERAU 4:e02ad2d9ba69 204 }
WoodyERAU 1:1d2b72d09c56 205 }
WoodyERAU 4:e02ad2d9ba69 206 else
WoodyERAU 4:e02ad2d9ba69 207 {
WoodyERAU 4:e02ad2d9ba69 208 front_thrust = 0.46;
WoodyERAU 4:e02ad2d9ba69 209 back_thrust = 0.46;
WoodyERAU 4:e02ad2d9ba69 210
WoodyERAU 4:e02ad2d9ba69 211 }
WoodyERAU 4:e02ad2d9ba69 212 }
WoodyERAU 1:1d2b72d09c56 213 }
WoodyERAU 1:1d2b72d09c56 214