Pat McC / Mbed 2 deprecated AUV_PIDusna

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BNO055_fusion_AUV

AUV_PID.cpp@0:37123f30e8b2, 2020-01-14 (annotated)

Committer:
pmmccorkell
Date:
Tue Jan 14 19:52:12 2020 +0000
Revision:
0:37123f30e8b2
Child:
3:d6471216e378
asdf

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmmccorkell 0:37123f30e8b2 1 #include "mbed.h"
pmmccorkell 0:37123f30e8b2 2 #include "BNO055.h" //imu
pmmccorkell 0:37123f30e8b2 3 #include "MS5837.h" //pressure sensor
pmmccorkell 0:37123f30e8b2 4 #include "PID.h"
pmmccorkell 0:37123f30e8b2 5
pmmccorkell 0:37123f30e8b2 6 //Setup USB Serial
pmmccorkell 0:37123f30e8b2 7 Serial pc(USBTX, USBRX);
pmmccorkell 0:37123f30e8b2 8 int baudrate = 115200;
pmmccorkell 0:37123f30e8b2 9
pmmccorkell 0:37123f30e8b2 10 //Setup BNO055 and MS5837 over I2C
pmmccorkell 0:37123f30e8b2 11 I2C i2c(p28,p27);
pmmccorkell 0:37123f30e8b2 12 DigitalOut pwr_on(p30);
pmmccorkell 0:37123f30e8b2 13 BNO055 imu(i2c,p8);
pmmccorkell 0:37123f30e8b2 14
pmmccorkell 0:37123f30e8b2 15 //instantiate globals for press sensor
pmmccorkell 0:37123f30e8b2 16 MS5837 press_sensor(I2C_SDA,I2C_SCL,ms5837_addr_no_CS);
pmmccorkell 0:37123f30e8b2 17 int press_sensor_type=1; //0 for 02BA, 1 for 30BA
pmmccorkell 0:37123f30e8b2 18 int sensor_rate=512; //Oversampling Rate, see data sheet
pmmccorkell 0:37123f30e8b2 19 float depth=0; //cm
pmmccorkell 0:37123f30e8b2 20
pmmccorkell 0:37123f30e8b2 21 int wait_main=20; //ms to wait in main, and associated, loops
pmmccorkell 0:37123f30e8b2 22
pmmccorkell 0:37123f30e8b2 23 /*
pmmccorkell 0:37123f30e8b2 24 // IO pins used with oscope to observe and gather timing data of the program itself
pmmccorkell 0:37123f30e8b2 25 DigitalOut function_timer(p5); //el logic + depth and pitch controllers
pmmccorkell 0:37123f30e8b2 26 DigitalOut function_timer2(p6); //sensor update and data stream
pmmccorkell 0:37123f30e8b2 27 DigitalOut function_timer3(p7); //az logic + heading and speed controllers
pmmccorkell 0:37123f30e8b2 28 DigitalOut function_timer4(p8); //high edge when a command is read and completed execution.
pmmccorkell 0:37123f30e8b2 29 //Pair with RasPi GPIO and use Oscope to time how long it
pmmccorkell 0:37123f30e8b2 30 //takes to send, receive, and execute commands.
pmmccorkell 0:37123f30e8b2 31 */
pmmccorkell 0:37123f30e8b2 32
pmmccorkell 0:37123f30e8b2 33 DigitalIn leak_detect(p11);
pmmccorkell 0:37123f30e8b2 34 DigitalOut leak_light(LED1);
pmmccorkell 0:37123f30e8b2 35
pmmccorkell 0:37123f30e8b2 36 // ESC specs data
pmmccorkell 0:37123f30e8b2 37 double esc_freq=400; //Standard servo rate -> 400Hz
pmmccorkell 0:37123f30e8b2 38 double base_pw=1.5; //ms
pmmccorkell 0:37123f30e8b2 39 double null_pw=0.0;
pmmccorkell 0:37123f30e8b2 40 double esc_period=(1000/esc_freq); //ms
pmmccorkell 0:37123f30e8b2 41 double esc_range_spec=.4; // 400ms, [1.1,1.9]ms pw per data sheet
pmmccorkell 0:37123f30e8b2 42 double esc_range_scale=1.0; //only use x% of full range
pmmccorkell 0:37123f30e8b2 43 double esc_range=esc_range_spec*esc_range_scale;
pmmccorkell 0:37123f30e8b2 44 double min_thruster_bias=0.028; //deadzone around 1.5ms where ESC will not operate
pmmccorkell 0:37123f30e8b2 45 double pw_tolerance=0.00001;
pmmccorkell 0:37123f30e8b2 46
pmmccorkell 0:37123f30e8b2 47
pmmccorkell 0:37123f30e8b2 48 volatile float ticker_rate= 0.02; //Time between ticker calls in seconds.
pmmccorkell 0:37123f30e8b2 49 float target_time=5; //Ideal timeframe to reach desired heading, depth, pitch using only Kp.
pmmccorkell 0:37123f30e8b2 50 float target_time_multiple=2; //Ki will max out PWM in "factor * target_time".
pmmccorkell 0:37123f30e8b2 51
pmmccorkell 0:37123f30e8b2 52 //
pmmccorkell 0:37123f30e8b2 53 //Azimuth controllers
pmmccorkell 0:37123f30e8b2 54 //
pmmccorkell 0:37123f30e8b2 55 Ticker tickerAzThrusters;
pmmccorkell 0:37123f30e8b2 56 //degrees of acceptable heading tolerance
pmmccorkell 0:37123f30e8b2 57 double az_tolerance=2;
pmmccorkell 0:37123f30e8b2 58 // PID gains for heading
pmmccorkell 0:37123f30e8b2 59 volatile float heading_Kp =0.0008333f;
pmmccorkell 0:37123f30e8b2 60 volatile float heading_Ki =0.0f;
pmmccorkell 0:37123f30e8b2 61 volatile float heading_Kd =0.0f;
pmmccorkell 0:37123f30e8b2 62 PID pid_heading(heading_Kp,heading_Ki,heading_Kd,ticker_rate,az_tolerance);
pmmccorkell 0:37123f30e8b2 63 // PID gains for speed
pmmccorkell 0:37123f30e8b2 64 // volatile float speed_Kp=1.0f;
pmmccorkell 0:37123f30e8b2 65 // volatile float speed_Ki=0.0f;
pmmccorkell 0:37123f30e8b2 66 // volatile float speed_Kd=0.0;
pmmccorkell 0:37123f30e8b2 67 // PID pid_speed(speed_Kp, speed_Ki, speed_Kd, ticker_rate, speed_tolerance);
pmmccorkell 0:37123f30e8b2 68
pmmccorkell 0:37123f30e8b2 69 //
pmmccorkell 0:37123f30e8b2 70 //Elevation controllers
pmmccorkell 0:37123f30e8b2 71 //
pmmccorkell 0:37123f30e8b2 72 Ticker tickerElThrusters;
pmmccorkell 0:37123f30e8b2 73 //acceptable depth tolerance in cm
pmmccorkell 0:37123f30e8b2 74 float depth_tolerance=0.2;
pmmccorkell 0:37123f30e8b2 75 // PID gains for depth
pmmccorkell 0:37123f30e8b2 76 volatile float depth_Kp =6.0f;
pmmccorkell 0:37123f30e8b2 77 volatile float depth_Ki =10.0f;
pmmccorkell 0:37123f30e8b2 78 volatile float depth_Kd =0.0;
pmmccorkell 0:37123f30e8b2 79 PID pid_depth(depth_Kp, depth_Ki, depth_Kd, ticker_rate, depth_tolerance);
pmmccorkell 0:37123f30e8b2 80 //acceptable pitch tolerance in degrees
pmmccorkell 0:37123f30e8b2 81 double pitch_tolerance=1;
pmmccorkell 0:37123f30e8b2 82 // PID gains for pitch
pmmccorkell 0:37123f30e8b2 83 volatile float pitch_Kp = 6.0f; //1.0f;
pmmccorkell 0:37123f30e8b2 84 volatile float pitch_Ki = 10.0f; //10.0f;
pmmccorkell 0:37123f30e8b2 85 volatile float pitch_Kd = 0.00001; //0.0;
pmmccorkell 0:37123f30e8b2 86 PID pid_pitch(pitch_Kp, pitch_Ki, pitch_Kd, ticker_rate, pitch_tolerance);
pmmccorkell 0:37123f30e8b2 87
pmmccorkell 0:37123f30e8b2 88 /*
pmmccorkell 0:37123f30e8b2 89 Ticker tickerRollThrusters;
pmmccorkell 0:37123f30e8b2 90 volatile float roll_Pk = .50f;
pmmccorkell 0:37123f30e8b2 91 volatile float roll_Ik = .10f;
pmmccorkell 0:37123f30e8b2 92 volatile float roll_Dk=0.0;
pmmccorkell 0:37123f30e8b2 93 PID pid_roll(roll_Pk, roll_Ik, roll_Dk, ticker_rate);
pmmccorkell 0:37123f30e8b2 94 volatile float strafe_Pk=.50f;
pmmccorkell 0:37123f30e8b2 95 volatile float strafe_Ik=.10f;
pmmccorkell 0:37123f30e8b2 96 volatile float strafe_Dk=0.0;
pmmccorkell 0:37123f30e8b2 97 PID pid_strafe(roll_Pk, roll_Ik, roll_Dk, ticker_rate);
pmmccorkell 0:37123f30e8b2 98 */
pmmccorkell 0:37123f30e8b2 99
pmmccorkell 0:37123f30e8b2 100 //instantiate globals for sensor updates and data stream
pmmccorkell 0:37123f30e8b2 101 uint16_t ready_prefix = 0x0000;
pmmccorkell 0:37123f30e8b2 102 uint16_t horizon_prefix=0xff00;
pmmccorkell 0:37123f30e8b2 103 uint16_t ready_data = 0x0000;
pmmccorkell 0:37123f30e8b2 104 uint16_t heading = 0xffff;
pmmccorkell 0:37123f30e8b2 105 uint16_t pitch = 0xffff;
pmmccorkell 0:37123f30e8b2 106 char readline[100];
pmmccorkell 0:37123f30e8b2 107
pmmccorkell 0:37123f30e8b2 108 //instantiate globals for flags and function indication
pmmccorkell 0:37123f30e8b2 109 int command_available=1;
pmmccorkell 0:37123f30e8b2 110 int call_threads_available=1;
pmmccorkell 0:37123f30e8b2 111 int logic_available=1;
pmmccorkell 0:37123f30e8b2 112 int manual_mode=0;
pmmccorkell 0:37123f30e8b2 113 int zero_set=0;
pmmccorkell 0:37123f30e8b2 114 int horizon_count=0;
pmmccorkell 0:37123f30e8b2 115 int event_horizon_flag=0;
pmmccorkell 0:37123f30e8b2 116
pmmccorkell 0:37123f30e8b2 117 //instantiate goto position globals
pmmccorkell 0:37123f30e8b2 118 //-1 indicates no setting
pmmccorkell 0:37123f30e8b2 119 int persistent_heading=-1;
pmmccorkell 0:37123f30e8b2 120 int persistent_speed=-1;
pmmccorkell 0:37123f30e8b2 121 int persistent_depth=-1;
pmmccorkell 0:37123f30e8b2 122 int persistent_pitch=-1;
pmmccorkell 0:37123f30e8b2 123
pmmccorkell 0:37123f30e8b2 124 BNO055_ID_INF_TypeDef bno055_id_inf;
pmmccorkell 0:37123f30e8b2 125 BNO055_EULER_TypeDef euler_angles;
pmmccorkell 0:37123f30e8b2 126 //BNO055_QUATERNION_TypeDef quaternion;
pmmccorkell 0:37123f30e8b2 127 //BNO055_LIN_ACC_TypeDef linear_acc;
pmmccorkell 0:37123f30e8b2 128 //BNO055_GRAVITY_TypeDef gravity;
pmmccorkell 0:37123f30e8b2 129 //BNO055_TEMPERATURE_TypeDef chip_temp;
pmmccorkell 0:37123f30e8b2 130
pmmccorkell 0:37123f30e8b2 131
pmmccorkell 0:37123f30e8b2 132 //-----THRUSTER CLASS BEGIN-----//
pmmccorkell 0:37123f30e8b2 133 //Thruster class to instantiate individual thrusters.
pmmccorkell 0:37123f30e8b2 134 class Thruster {
pmmccorkell 0:37123f30e8b2 135 public:
pmmccorkell 0:37123f30e8b2 136 Thruster(PinName pin, float dir);
pmmccorkell 0:37123f30e8b2 137 void setEvent();
pmmccorkell 0:37123f30e8b2 138 void clearEvent();
pmmccorkell 0:37123f30e8b2 139 int available();
pmmccorkell 0:37123f30e8b2 140 void set_period(double thruster_time);
pmmccorkell 0:37123f30e8b2 141 void set_pw(double thruster_pw);
pmmccorkell 0:37123f30e8b2 142 double get_pw();
pmmccorkell 0:37123f30e8b2 143 double get_speed();
pmmccorkell 0:37123f30e8b2 144 uint32_t thruster_data();
pmmccorkell 0:37123f30e8b2 145 void set_speed(double pntr);
pmmccorkell 0:37123f30e8b2 146 protected:
pmmccorkell 0:37123f30e8b2 147 PwmOut _pwm;
pmmccorkell 0:37123f30e8b2 148 PinName _pin;
pmmccorkell 0:37123f30e8b2 149 float _d;
pmmccorkell 0:37123f30e8b2 150 int _lock;
pmmccorkell 0:37123f30e8b2 151 int _available;
pmmccorkell 0:37123f30e8b2 152 double _base_pw, _period;
pmmccorkell 0:37123f30e8b2 153 };
pmmccorkell 0:37123f30e8b2 154
pmmccorkell 0:37123f30e8b2 155 //Instantiation accepts PWM pin and direction
pmmccorkell 0:37123f30e8b2 156 //Direction is -1 or 1. 1 for normal, -1 if blade reversed.
pmmccorkell 0:37123f30e8b2 157 Thruster::Thruster(PinName pin, float dir) : _pwm(pin), _d(dir) {
pmmccorkell 0:37123f30e8b2 158 _lock=0;
pmmccorkell 0:37123f30e8b2 159 _available=1;
pmmccorkell 0:37123f30e8b2 160 _pin=pin;
pmmccorkell 0:37123f30e8b2 161 _base_pw=1.5;
pmmccorkell 0:37123f30e8b2 162 set_pw(_base_pw);
pmmccorkell 0:37123f30e8b2 163 _period=2.5;
pmmccorkell 0:37123f30e8b2 164 set_period(_period);
pmmccorkell 0:37123f30e8b2 165 //pc.printf("Thruster: %f\r\n",d);
pmmccorkell 0:37123f30e8b2 166 }
pmmccorkell 0:37123f30e8b2 167
pmmccorkell 0:37123f30e8b2 168 //Sets Event for Emergency Stop and sets lockout to set_speed() function.
pmmccorkell 0:37123f30e8b2 169 void Thruster::setEvent() {
pmmccorkell 0:37123f30e8b2 170 _lock=1;
pmmccorkell 0:37123f30e8b2 171 set_pw(_base_pw);
pmmccorkell 0:37123f30e8b2 172 }
pmmccorkell 0:37123f30e8b2 173
pmmccorkell 0:37123f30e8b2 174 //Clears Event for Emergency Stop of thruster and removes lockout from set_speed() function.
pmmccorkell 0:37123f30e8b2 175 void Thruster::clearEvent() {
pmmccorkell 0:37123f30e8b2 176 _lock=0;
pmmccorkell 0:37123f30e8b2 177 }
pmmccorkell 0:37123f30e8b2 178
pmmccorkell 0:37123f30e8b2 179 //Returns whether set_speed() function is available, or currently in use.
pmmccorkell 0:37123f30e8b2 180 int Thruster::available() {
pmmccorkell 0:37123f30e8b2 181 return _available;
pmmccorkell 0:37123f30e8b2 182 }
pmmccorkell 0:37123f30e8b2 183
pmmccorkell 0:37123f30e8b2 184 //Set PWM period in ms.
pmmccorkell 0:37123f30e8b2 185 void Thruster::set_period(double thruster_time) {
pmmccorkell 0:37123f30e8b2 186 _period=thruster_time;
pmmccorkell 0:37123f30e8b2 187 _pwm.period(_period/1000);
pmmccorkell 0:37123f30e8b2 188 }
pmmccorkell 0:37123f30e8b2 189
pmmccorkell 0:37123f30e8b2 190 //Set PWM pulsewidth in ms
pmmccorkell 0:37123f30e8b2 191 void Thruster::set_pw(double thruster_pw) {
pmmccorkell 0:37123f30e8b2 192 double s_pw=(thruster_pw/1000);
pmmccorkell 0:37123f30e8b2 193 pc.printf("log: set_pw: %f\r\n",s_pw);
pmmccorkell 0:37123f30e8b2 194 _pwm.pulsewidth(s_pw);
pmmccorkell 0:37123f30e8b2 195 }
pmmccorkell 0:37123f30e8b2 196
pmmccorkell 0:37123f30e8b2 197 //Returns PWM pulsewidth in ms.
pmmccorkell 0:37123f30e8b2 198 double Thruster::get_pw() {
pmmccorkell 0:37123f30e8b2 199 //read duty cycle times period
pmmccorkell 0:37123f30e8b2 200 double g_pw = (_pwm.read()*_period);
pmmccorkell 0:37123f30e8b2 201 //pc.printf(" get_pw: %f, ",g_pw);
pmmccorkell 0:37123f30e8b2 202 return g_pw;
pmmccorkell 0:37123f30e8b2 203 }
pmmccorkell 0:37123f30e8b2 204
pmmccorkell 0:37123f30e8b2 205 //Returns PWM output relative to 1.5ms.
pmmccorkell 0:37123f30e8b2 206 double Thruster::get_speed() {
pmmccorkell 0:37123f30e8b2 207 double g_speed = (get_pw()-_base_pw);
pmmccorkell 0:37123f30e8b2 208 //pc.printf("get_speed: %f, ",g_speed);
pmmccorkell 0:37123f30e8b2 209 return g_speed;
pmmccorkell 0:37123f30e8b2 210 }
pmmccorkell 0:37123f30e8b2 211
pmmccorkell 0:37123f30e8b2 212 //formats PWM as an 2 uint16_t joined to make uint32_t for serial data streaming
pmmccorkell 0:37123f30e8b2 213 //MSB uint16_t indicates direction, 0 for positive, 1 for negative.
pmmccorkell 0:37123f30e8b2 214 //LSB uint16_t is 10ms resolution of PWM
pmmccorkell 0:37123f30e8b2 215 uint32_t Thruster::thruster_data() {
pmmccorkell 0:37123f30e8b2 216 double speed=get_speed();
pmmccorkell 0:37123f30e8b2 217 uint32_t dir=0x0;
pmmccorkell 0:37123f30e8b2 218 uint32_t data=0x0;
pmmccorkell 0:37123f30e8b2 219 if (speed<0) dir =0x00010000;
pmmccorkell 0:37123f30e8b2 220 data=static_cast<unsigned int>(abs(int(speed*100000)));
pmmccorkell 0:37123f30e8b2 221 data=data+dir;
pmmccorkell 0:37123f30e8b2 222 return data;
pmmccorkell 0:37123f30e8b2 223 }
pmmccorkell 0:37123f30e8b2 224
pmmccorkell 0:37123f30e8b2 225 //Accepts adjustment to pw [-500,500] ms that is added to 1.5ms
pmmccorkell 0:37123f30e8b2 226 void Thruster::set_speed(double speed_pw) {
pmmccorkell 0:37123f30e8b2 227 if (_lock==1) {
pmmccorkell 0:37123f30e8b2 228 set_pw(_base_pw);
pmmccorkell 0:37123f30e8b2 229 }
pmmccorkell 0:37123f30e8b2 230 else {
pmmccorkell 0:37123f30e8b2 231 double tolerance_pw=0.001;
pmmccorkell 0:37123f30e8b2 232 double target_pw=(_d*speed_pw)+_base_pw;
pmmccorkell 0:37123f30e8b2 233 double current_pw=get_pw();
pmmccorkell 0:37123f30e8b2 234 double diff_pw=abs(target_pw-current_pw);
pmmccorkell 0:37123f30e8b2 235 if (diff_pw>tolerance_pw) set_pw(target_pw);
pmmccorkell 0:37123f30e8b2 236 }
pmmccorkell 0:37123f30e8b2 237 }
pmmccorkell 0:37123f30e8b2 238 //-----THRUSTER CLASS END-----//
pmmccorkell 0:37123f30e8b2 239
pmmccorkell 0:37123f30e8b2 240 // Instantiate thrusters.
pmmccorkell 0:37123f30e8b2 241 Thruster port_thrust(p21,-1);
pmmccorkell 0:37123f30e8b2 242 Thruster starboard_thrust(p22,1);
pmmccorkell 0:37123f30e8b2 243 //Thruster steadystate(p23,1); //for test purposes, to keep ESC from making LOUD NOISES
pmmccorkell 0:37123f30e8b2 244 Thruster fore_thrust(p24,1);
pmmccorkell 0:37123f30e8b2 245 Thruster aft_thrust(p25,-1);
pmmccorkell 0:37123f30e8b2 246
pmmccorkell 0:37123f30e8b2 247 //Function to check for water leak. Open is good, short is bad.
pmmccorkell 0:37123f30e8b2 248 void leak_data() {
pmmccorkell 0:37123f30e8b2 249 leak_detect.read();
pmmccorkell 0:37123f30e8b2 250 if (leak_detect==1) {
pmmccorkell 0:37123f30e8b2 251 ready_data=(ready_data | 0x0800);
pmmccorkell 0:37123f30e8b2 252 }
pmmccorkell 0:37123f30e8b2 253 leak_light=leak_detect;
pmmccorkell 0:37123f30e8b2 254 }
pmmccorkell 0:37123f30e8b2 255
pmmccorkell 0:37123f30e8b2 256 //Function to get elevation data and send to RasPi.
pmmccorkell 0:37123f30e8b2 257 uint32_t el_data() {
pmmccorkell 0:37123f30e8b2 258 //Run Barometric equations from pressure sensor.
pmmccorkell 0:37123f30e8b2 259 press_sensor.calculate();
pmmccorkell 0:37123f30e8b2 260 depth=press_sensor.depth();
pmmccorkell 0:37123f30e8b2 261 uint32_t depth_data=(depth*0x20);
pmmccorkell 0:37123f30e8b2 262 //0xb0 acts as prefix to identify Barometer Pressure.
pmmccorkell 0:37123f30e8b2 263 //Pressure sensor sends pressure in range [0x3e8,0x1d4c0]. Divide by 100 for mbar.
pmmccorkell 0:37123f30e8b2 264 uint32_t el_data_comp=(0xb1000000|depth_data);
pmmccorkell 0:37123f30e8b2 265 return el_data_comp;
pmmccorkell 0:37123f30e8b2 266 }
pmmccorkell 0:37123f30e8b2 267
pmmccorkell 0:37123f30e8b2 268 //Function to format Gain values into 3 significant digits and exponent value.
pmmccorkell 0:37123f30e8b2 269 // ie 0x abc * 10^(d-9) -> 0x abcd
pmmccorkell 0:37123f30e8b2 270 uint16_t k_data(float k_val) {
pmmccorkell 0:37123f30e8b2 271
pmmccorkell 0:37123f30e8b2 272 // Static lookup table for 10^11 to 10^-4.
pmmccorkell 0:37123f30e8b2 273 // Arrived at for range to get 3 significant digits for values from 10^-9 to 10^6, respectively.
pmmccorkell 0:37123f30e8b2 274 static double pow10[16] = {
pmmccorkell 0:37123f30e8b2 275 100000000000, 10000000000, 1000000000, 100000000, //[0,4]
pmmccorkell 0:37123f30e8b2 276 10000000, 1000000, 100000, 10000, //[5,8]
pmmccorkell 0:37123f30e8b2 277 1000, 100, 10, 1, //[9,12]
pmmccorkell 0:37123f30e8b2 278 0.1, 0.01, 0.001, 0.0001, //[13,16]
pmmccorkell 0:37123f30e8b2 279 };
pmmccorkell 0:37123f30e8b2 280
pmmccorkell 0:37123f30e8b2 281 // Find where significant digits start and get the exponent value.
pmmccorkell 0:37123f30e8b2 282 int exponent_value = floor(log10(k_val));
pmmccorkell 0:37123f30e8b2 283
pmmccorkell 0:37123f30e8b2 284 // Form scalar value of 3 significant digits.
pmmccorkell 0:37123f30e8b2 285 int sig_val = floor((k_val*pow10[exponent_value+9])+0.5);
pmmccorkell 0:37123f30e8b2 286
pmmccorkell 0:37123f30e8b2 287 // Shift exponent so that 10^-9 starts at 0.
pmmccorkell 0:37123f30e8b2 288 exponent_value+=9;
pmmccorkell 0:37123f30e8b2 289
pmmccorkell 0:37123f30e8b2 290 // Move Scalar left 1 full hex, and append exponent to the end.
pmmccorkell 0:37123f30e8b2 291 uint16_t return_val=(sig_val<<4)+(exponent_value);
pmmccorkell 0:37123f30e8b2 292
pmmccorkell 0:37123f30e8b2 293 //Return the formed hex value.
pmmccorkell 0:37123f30e8b2 294 return return_val;
pmmccorkell 0:37123f30e8b2 295 }
pmmccorkell 0:37123f30e8b2 296
pmmccorkell 0:37123f30e8b2 297 //Data function pulls data from BNO and sends over serial
pmmccorkell 0:37123f30e8b2 298 //Timed using DigitalOut and Oscope.
pmmccorkell 0:37123f30e8b2 299 // At baud 115200, averaged 5 times over 256, +pulsewidth 11.1 - 13.3ms.
pmmccorkell 0:37123f30e8b2 300 // At baud 921600, averaged over 256, +pw 4.1 - 5.5ms
pmmccorkell 0:37123f30e8b2 301 // In water, 921600 induced serial comm errors
pmmccorkell 0:37123f30e8b2 302 // Variance is due to MS5837 pressure sensor. Includes waits of 2-4ms.
pmmccorkell 0:37123f30e8b2 303 void az_data() {
pmmccorkell 0:37123f30e8b2 304 //function_timer2=1;
pmmccorkell 0:37123f30e8b2 305
pmmccorkell 0:37123f30e8b2 306 leak_data();
pmmccorkell 0:37123f30e8b2 307 uint32_t k=0x1234abcd;
pmmccorkell 0:37123f30e8b2 308
pmmccorkell 0:37123f30e8b2 309 // if (logic_available==1) ready_data=(ready_data&0xfbff);
pmmccorkell 0:37123f30e8b2 310 // else ready_data=(ready_data|0x0400);
pmmccorkell 0:37123f30e8b2 311 if (call_threads_available==1) ready_data=(ready_data & 0xfdff);
pmmccorkell 0:37123f30e8b2 312 else ready_data=(ready_data | 0x0200);
pmmccorkell 0:37123f30e8b2 313 // if (command_available==1) ready_data=(ready_data&0xfeff);
pmmccorkell 0:37123f30e8b2 314 // else ready_data=(ready_data | 0x0100);
pmmccorkell 0:37123f30e8b2 315 if (zero_set==1) ready_data=(ready_data|0x0008);
pmmccorkell 0:37123f30e8b2 316 else ready_data=(ready_data&0xfff7);
pmmccorkell 0:37123f30e8b2 317
pmmccorkell 0:37123f30e8b2 318 //Instantiate status array of 7 32-bit words.
pmmccorkell 0:37123f30e8b2 319 //First 16 bits of each 32-bit word are Identifiers for RasPi to correctly assign the trailing 16 bits of data.
pmmccorkell 0:37123f30e8b2 320 uint32_t status[14]={0};
pmmccorkell 0:37123f30e8b2 321 uint32_t ready=ready_prefix;
pmmccorkell 0:37123f30e8b2 322 ready=(ready<<16)|ready_data;
pmmccorkell 0:37123f30e8b2 323
pmmccorkell 0:37123f30e8b2 324 //word 0: Key
pmmccorkell 0:37123f30e8b2 325 //Used to ensure Pi and Mbed are on same page.
pmmccorkell 0:37123f30e8b2 326 status[0]=k;
pmmccorkell 0:37123f30e8b2 327
pmmccorkell 0:37123f30e8b2 328 //word 1: Status information.
pmmccorkell 0:37123f30e8b2 329 //0xffff acts as prefix to identify Status for RasPi.
pmmccorkell 0:37123f30e8b2 330 //Last 3 bits (from right) are current position (POS[0-7]). See BNO datasheet.
pmmccorkell 0:37123f30e8b2 331 //4th bit (from right) is RH turn motors enabled.
pmmccorkell 0:37123f30e8b2 332 //5th bit (from right) is LH turn motors enabled.
pmmccorkell 0:37123f30e8b2 333 status[1]=ready;
pmmccorkell 0:37123f30e8b2 334
pmmccorkell 0:37123f30e8b2 335 //word 2: Calibration.
pmmccorkell 0:37123f30e8b2 336 //0xc000 acts as prefix to identify Cal for RasPi.
pmmccorkell 0:37123f30e8b2 337 status[2]=0xc0000000+imu.read_calib_status();
pmmccorkell 0:37123f30e8b2 338
pmmccorkell 0:37123f30e8b2 339 //Get Euler data from BNO.
pmmccorkell 0:37123f30e8b2 340 imu.get_Euler_Angles(&euler_angles);
pmmccorkell 0:37123f30e8b2 341
pmmccorkell 0:37123f30e8b2 342 //word 3 is Heading.
pmmccorkell 0:37123f30e8b2 343 //0xc100 acts as prefix to identify Heading for RasPi.
pmmccorkell 0:37123f30e8b2 344 uint16_t h = euler_angles.h;
pmmccorkell 0:37123f30e8b2 345 heading=h;
pmmccorkell 0:37123f30e8b2 346 status[3]=0xc1000000+h;
pmmccorkell 0:37123f30e8b2 347
pmmccorkell 0:37123f30e8b2 348 //Offset calculation: 360*16bit resolution = 5760 -> converted to hex = 0x1680
pmmccorkell 0:37123f30e8b2 349 int offset=0x1680;
pmmccorkell 0:37123f30e8b2 350
pmmccorkell 0:37123f30e8b2 351 //word 4 is Roll.
pmmccorkell 0:37123f30e8b2 352 //0xc300 acts as prefix to identify Roll for RasPi.
pmmccorkell 0:37123f30e8b2 353 //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
pmmccorkell 0:37123f30e8b2 354 uint16_t r = offset + euler_angles.r;
pmmccorkell 0:37123f30e8b2 355 status[4]=0xc3000000+r;
pmmccorkell 0:37123f30e8b2 356
pmmccorkell 0:37123f30e8b2 357 //word 5 is Pitch.
pmmccorkell 0:37123f30e8b2 358 //0xc500 acts as prefix to identify Pitch for RasPi.
pmmccorkell 0:37123f30e8b2 359 //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
pmmccorkell 0:37123f30e8b2 360 uint16_t p = offset + euler_angles.p;
pmmccorkell 0:37123f30e8b2 361 pitch=(p - (offset/2)); //only want 180deg offset
pmmccorkell 0:37123f30e8b2 362 status[5]=0xc5000000+p;
pmmccorkell 0:37123f30e8b2 363
pmmccorkell 0:37123f30e8b2 364 //word 6 gets Depth from el_data() function.
pmmccorkell 0:37123f30e8b2 365 //0xb0 acts as prefix to identify Barometer Pressure.
pmmccorkell 0:37123f30e8b2 366 status[6]=el_data();
pmmccorkell 0:37123f30e8b2 367
pmmccorkell 0:37123f30e8b2 368 //Thruster speeds in 10us resolution.
pmmccorkell 0:37123f30e8b2 369 status[7]=((port_thrust.thruster_data() & 0x00ffffff) | 0xf1000000);
pmmccorkell 0:37123f30e8b2 370 status[8]=((starboard_thrust.thruster_data() &0x00ffffff) | 0xf2000000);
pmmccorkell 0:37123f30e8b2 371 status[9]=((fore_thrust.thruster_data() & 0x00ffffff) | 0xf3000000);
pmmccorkell 0:37123f30e8b2 372 status[10]=((aft_thrust.thruster_data() & 0x00ffffff) | 0xf4000000);
pmmccorkell 0:37123f30e8b2 373
pmmccorkell 0:37123f30e8b2 374 //Gains in format: ABC * 10^(D-9), similar to resistor color bands but starting at 10^-9 rather than 10^0.
pmmccorkell 0:37123f30e8b2 375 //Value is sent in Hex, but the 3bit scalar, ABC, is decimal [0,999]. Exponent component, D, may be hex to obtain range of [10^-9,10^6]
pmmccorkell 0:37123f30e8b2 376
pmmccorkell 0:37123f30e8b2 377 //Heading Gains
pmmccorkell 0:37123f30e8b2 378 status[11]= 0xd1100000 + k_data(heading_Kp); //Kp
pmmccorkell 0:37123f30e8b2 379 // status[12]= 0xd1200000 + k_data(heading_Ki); //Ki
pmmccorkell 0:37123f30e8b2 380 // status[13]= 0xd1300000 + k_data(heading_Kd); //Kd
pmmccorkell 0:37123f30e8b2 381
pmmccorkell 0:37123f30e8b2 382 //Pitch Gains
pmmccorkell 0:37123f30e8b2 383 status[12]= 0xd1200000 + k_data(pitch_Kp); //Kp
pmmccorkell 0:37123f30e8b2 384 // status[15]= 0xd5200000 + k_data(pitch_Ki); //Ki
pmmccorkell 0:37123f30e8b2 385 // status[16]= 0xd5300000 + k_data(pitch_Kd); //Kd
pmmccorkell 0:37123f30e8b2 386
pmmccorkell 0:37123f30e8b2 387 //Depth Gains
pmmccorkell 0:37123f30e8b2 388 status[13]= 0xd1300000 + k_data(depth_Kp); //Kp
pmmccorkell 0:37123f30e8b2 389 // status[18]= 0xd7200000 + k_data(depth_Ki); //Ki
pmmccorkell 0:37123f30e8b2 390 // status[19]= 0xd7300000 + k_data(depth_Kd); //Kd
pmmccorkell 0:37123f30e8b2 391
pmmccorkell 0:37123f30e8b2 392 //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi.
pmmccorkell 0:37123f30e8b2 393 int i;
pmmccorkell 0:37123f30e8b2 394 int l=(sizeof(status)/sizeof(uint32_t))-1;
pmmccorkell 0:37123f30e8b2 395 for (i=0; i<=l; i++) {
pmmccorkell 0:37123f30e8b2 396 pc.printf("%x\n", status[i]);
pmmccorkell 0:37123f30e8b2 397 }
pmmccorkell 0:37123f30e8b2 398 //function_timer2=0;
pmmccorkell 0:37123f30e8b2 399 }
pmmccorkell 0:37123f30e8b2 400
pmmccorkell 0:37123f30e8b2 401 //reset all controller set points and zero out any accumulated integral gains to ESCs
pmmccorkell 0:37123f30e8b2 402 void stop_all_persistent() {
pmmccorkell 0:37123f30e8b2 403 persistent_heading=-1;
pmmccorkell 0:37123f30e8b2 404 persistent_speed=-1;
pmmccorkell 0:37123f30e8b2 405 persistent_depth=-1;
pmmccorkell 0:37123f30e8b2 406 persistent_pitch=-1;
pmmccorkell 0:37123f30e8b2 407 pid_pitch.clear();
pmmccorkell 0:37123f30e8b2 408 pid_depth.clear();
pmmccorkell 0:37123f30e8b2 409 pid_heading.clear();
pmmccorkell 0:37123f30e8b2 410 // pid_speed.reset();
pmmccorkell 0:37123f30e8b2 411 }
pmmccorkell 0:37123f30e8b2 412
pmmccorkell 0:37123f30e8b2 413 //Mostly deprecated by xyzControllers
pmmccorkell 0:37123f30e8b2 414 //Function to create new threads for motor logic
pmmccorkell 0:37123f30e8b2 415 void call_threads(int select, double dir=1, double speed=0) {
pmmccorkell 0:37123f30e8b2 416 call_threads_available=0;
pmmccorkell 0:37123f30e8b2 417 double rev_speed=(-1*speed);
pmmccorkell 0:37123f30e8b2 418 //Masking for port and starboard thruster status.
pmmccorkell 0:37123f30e8b2 419 uint16_t ready_mask_ps=0xff3f;
pmmccorkell 0:37123f30e8b2 420 //Masking for fore and aft thruster status.
pmmccorkell 0:37123f30e8b2 421 uint16_t ready_mask_fa=0xffcf;
pmmccorkell 0:37123f30e8b2 422 switch (select) {
pmmccorkell 0:37123f30e8b2 423 //case 1, forwards or backwards
pmmccorkell 0:37123f30e8b2 424 case 1:
pmmccorkell 0:37123f30e8b2 425 ready_data=(ready_data&ready_mask_ps)|0x00c0;
pmmccorkell 0:37123f30e8b2 426 if (dir==1) {
pmmccorkell 0:37123f30e8b2 427 pc.printf("log: call_threads Fwd, %f\r\n",speed);
pmmccorkell 0:37123f30e8b2 428 starboard_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 429 port_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 430 }
pmmccorkell 0:37123f30e8b2 431 if (dir==-1) {
pmmccorkell 0:37123f30e8b2 432 pc.printf("log: call_threads Rev, %f\r\n",speed);
pmmccorkell 0:37123f30e8b2 433 starboard_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 434 port_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 435 }
pmmccorkell 0:37123f30e8b2 436 break;
pmmccorkell 0:37123f30e8b2 437
pmmccorkell 0:37123f30e8b2 438 //case 2, turn left or right
pmmccorkell 0:37123f30e8b2 439 case 2:
pmmccorkell 0:37123f30e8b2 440 ready_data=(ready_data&ready_mask_ps)|0x00c0;
pmmccorkell 0:37123f30e8b2 441 if (dir==1) {
pmmccorkell 0:37123f30e8b2 442 pc.printf("log: call_threads Turn R, %f\r\n",speed);
pmmccorkell 0:37123f30e8b2 443 starboard_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 444 port_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 445 }
pmmccorkell 0:37123f30e8b2 446 if (dir==-1) {
pmmccorkell 0:37123f30e8b2 447 pc.printf("log: call_threads Turn L, %f\r\n",speed);
pmmccorkell 0:37123f30e8b2 448 starboard_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 449 port_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 450 }
pmmccorkell 0:37123f30e8b2 451 break;
pmmccorkell 0:37123f30e8b2 452
pmmccorkell 0:37123f30e8b2 453 //case 3, Up and Down
pmmccorkell 0:37123f30e8b2 454 case 3:
pmmccorkell 0:37123f30e8b2 455 ready_data=(ready_data&ready_mask_fa)|0x0030;
pmmccorkell 0:37123f30e8b2 456 if (dir==1) {
pmmccorkell 0:37123f30e8b2 457 pc.printf("log: call_threads Up, %f\r\n",speed);
pmmccorkell 0:37123f30e8b2 458 fore_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 459 aft_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 460 }
pmmccorkell 0:37123f30e8b2 461 if (dir==-1) {
pmmccorkell 0:37123f30e8b2 462 pc.printf("log:call_threads Down,%f\r\n",speed);
pmmccorkell 0:37123f30e8b2 463 fore_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 464 aft_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 465 }
pmmccorkell 0:37123f30e8b2 466 break;
pmmccorkell 0:37123f30e8b2 467
pmmccorkell 0:37123f30e8b2 468 //case 4, pitch up/down
pmmccorkell 0:37123f30e8b2 469 case 4:
pmmccorkell 0:37123f30e8b2 470 ready_data=(ready_data&ready_mask_fa)|0x0030;
pmmccorkell 0:37123f30e8b2 471 if (dir==1) {
pmmccorkell 0:37123f30e8b2 472 pc.printf("log: call_threads Pitch Up,%f\r\n",speed);
pmmccorkell 0:37123f30e8b2 473 fore_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 474 aft_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 475 }
pmmccorkell 0:37123f30e8b2 476 if (dir==-1) {
pmmccorkell 0:37123f30e8b2 477 pc.printf("log: call_threads Pitch Down,%f\r\n",speed);
pmmccorkell 0:37123f30e8b2 478 fore_thrust.set_speed(rev_speed);
pmmccorkell 0:37123f30e8b2 479 aft_thrust.set_speed(speed);
pmmccorkell 0:37123f30e8b2 480 }
pmmccorkell 0:37123f30e8b2 481 break;
pmmccorkell 0:37123f30e8b2 482
pmmccorkell 0:37123f30e8b2 483 //cases 5 and 6 reserved for roll should we add more thrusters.
pmmccorkell 0:37123f30e8b2 484
pmmccorkell 0:37123f30e8b2 485 //case 77, Emergency Surface
pmmccorkell 0:37123f30e8b2 486 case 77:
pmmccorkell 0:37123f30e8b2 487 pc.printf("log: call_threads Emergency Surface,%f\r\n",speed);
pmmccorkell 0:37123f30e8b2 488 //Fore and Aft up: 111
pmmccorkell 0:37123f30e8b2 489 ready_data=(ready_data&ready_mask_fa)|0x0030;
pmmccorkell 0:37123f30e8b2 490 fore_thrust.set_speed(esc_range_spec);
pmmccorkell 0:37123f30e8b2 491 aft_thrust.set_speed(esc_range_spec);
pmmccorkell 0:37123f30e8b2 492 break;
pmmccorkell 0:37123f30e8b2 493
pmmccorkell 0:37123f30e8b2 494 //case 99, Stop Fore and Aft thrusters.
pmmccorkell 0:37123f30e8b2 495 case 99:
pmmccorkell 0:37123f30e8b2 496 pc.printf("log: Stop el thrusters,%f\r\n",null_pw);
pmmccorkell 0:37123f30e8b2 497 //fore and aft thrusters stopped: 000
pmmccorkell 0:37123f30e8b2 498 ready_data=(ready_data&ready_mask_fa);
pmmccorkell 0:37123f30e8b2 499 fore_thrust.set_speed(null_pw);
pmmccorkell 0:37123f30e8b2 500 aft_thrust.set_speed(null_pw);
pmmccorkell 0:37123f30e8b2 501 break;
pmmccorkell 0:37123f30e8b2 502
pmmccorkell 0:37123f30e8b2 503 //case 0, stop az thrusters
pmmccorkell 0:37123f30e8b2 504 default:
pmmccorkell 0:37123f30e8b2 505 case 0:
pmmccorkell 0:37123f30e8b2 506 pc.printf("log: Stop az thrusters,%f\r\n",null_pw);
pmmccorkell 0:37123f30e8b2 507 //starboard and port thrusters stopped: 000
pmmccorkell 0:37123f30e8b2 508 ready_data=(ready_data&ready_mask_ps);
pmmccorkell 0:37123f30e8b2 509 starboard_thrust.set_speed(null_pw);
pmmccorkell 0:37123f30e8b2 510 port_thrust.set_speed(null_pw);
pmmccorkell 0:37123f30e8b2 511 break;
pmmccorkell 0:37123f30e8b2 512 }
pmmccorkell 0:37123f30e8b2 513 call_threads_available=1;
pmmccorkell 0:37123f30e8b2 514 }
pmmccorkell 0:37123f30e8b2 515
pmmccorkell 0:37123f30e8b2 516 //When bad things are happening.
pmmccorkell 0:37123f30e8b2 517 void EventHorizon() {
pmmccorkell 0:37123f30e8b2 518 event_horizon_flag=1;
pmmccorkell 0:37123f30e8b2 519 stop_all_persistent();
pmmccorkell 0:37123f30e8b2 520 horizon_count++;
pmmccorkell 0:37123f30e8b2 521 pc.printf("log: EventHorizon called, count: %i\r\n",horizon_count);
pmmccorkell 0:37123f30e8b2 522 //setEvent() method locks out Thruster class set_speed() function
pmmccorkell 0:37123f30e8b2 523 // and sets PWM to 1.5ms.
pmmccorkell 0:37123f30e8b2 524 port_thrust.setEvent();
pmmccorkell 0:37123f30e8b2 525 starboard_thrust.setEvent();
pmmccorkell 0:37123f30e8b2 526 fore_thrust.setEvent();
pmmccorkell 0:37123f30e8b2 527 aft_thrust.setEvent();
pmmccorkell 0:37123f30e8b2 528 pc.printf("log: Thruster events successfully set\r\n");
pmmccorkell 0:37123f30e8b2 529 //Tells Raspi that Emergency state has been initiated.
pmmccorkell 0:37123f30e8b2 530 ready_prefix=(horizon_prefix+horizon_count);
pmmccorkell 0:37123f30e8b2 531 //Wait some time during which Thruster set_speed() functions are locked out.
pmmccorkell 0:37123f30e8b2 532 for (int i=0; i<200; i++) {
pmmccorkell 0:37123f30e8b2 533 //Resume streaming data to RasPi during timeout period.
pmmccorkell 0:37123f30e8b2 534 az_data();
pmmccorkell 0:37123f30e8b2 535 wait_ms(wait_main);
pmmccorkell 0:37123f30e8b2 536 }
pmmccorkell 0:37123f30e8b2 537 //Clear emergency situation.
pmmccorkell 0:37123f30e8b2 538 port_thrust.clearEvent();
pmmccorkell 0:37123f30e8b2 539 starboard_thrust.clearEvent();
pmmccorkell 0:37123f30e8b2 540 fore_thrust.clearEvent();
pmmccorkell 0:37123f30e8b2 541 aft_thrust.clearEvent();
pmmccorkell 0:37123f30e8b2 542 pc.printf("log: Thruster events successfully cleared\r\n");
pmmccorkell 0:37123f30e8b2 543 //Set PWM to 1.5ms after emergency situation. Should have been set to 1.5ms, but double checking.
pmmccorkell 0:37123f30e8b2 544 // For extra precaution.
pmmccorkell 0:37123f30e8b2 545 stop_all_persistent();
pmmccorkell 0:37123f30e8b2 546 call_threads(0);
pmmccorkell 0:37123f30e8b2 547 call_threads(99);
pmmccorkell 0:37123f30e8b2 548 //Lets Raspi know that Mbed is ready for commands again.
pmmccorkell 0:37123f30e8b2 549 ready_prefix=0xffff;
pmmccorkell 0:37123f30e8b2 550 pc.printf("log: ready status reset, mbed may resume\r\n");
pmmccorkell 0:37123f30e8b2 551 event_horizon_flag=0;
pmmccorkell 0:37123f30e8b2 552 }
pmmccorkell 0:37123f30e8b2 553
pmmccorkell 0:37123f30e8b2 554 //Commands function handles Serial input, checks for correct syntax, and calls appropriate function(s) to execute commands.
pmmccorkell 0:37123f30e8b2 555 int read_serial() {
pmmccorkell 0:37123f30e8b2 556 int i=0;
pmmccorkell 0:37123f30e8b2 557 while (pc.readable()) {
pmmccorkell 0:37123f30e8b2 558 readline[i]=pc.getc();
pmmccorkell 0:37123f30e8b2 559 pc.printf("log: input read %c at %i\r\n",readline[i],i);
pmmccorkell 0:37123f30e8b2 560 i++;
pmmccorkell 0:37123f30e8b2 561 }
pmmccorkell 0:37123f30e8b2 562 //pc.printf("i: %i\r\n",i);
pmmccorkell 0:37123f30e8b2 563 return i;
pmmccorkell 0:37123f30e8b2 564 }
pmmccorkell 0:37123f30e8b2 565 int look_horizon() {
pmmccorkell 0:37123f30e8b2 566 int returnval=0;
pmmccorkell 0:37123f30e8b2 567 pc.printf("log: THREAD START, horizon\r\n");
pmmccorkell 0:37123f30e8b2 568 //int length=0;
pmmccorkell 0:37123f30e8b2 569 //if (length==4) {
pmmccorkell 0:37123f30e8b2 570 char check_HORIZON[5]="STOP";
pmmccorkell 0:37123f30e8b2 571 int verified_HORIZON=0;
pmmccorkell 0:37123f30e8b2 572 for (int i=0; i<5; i++) {
pmmccorkell 0:37123f30e8b2 573 if (readline[i]==check_HORIZON[i]) verified_HORIZON++;
pmmccorkell 0:37123f30e8b2 574 }
pmmccorkell 0:37123f30e8b2 575 if (verified_HORIZON==4) {
pmmccorkell 0:37123f30e8b2 576 EventHorizon();
pmmccorkell 0:37123f30e8b2 577 returnval=1;
pmmccorkell 0:37123f30e8b2 578 }
pmmccorkell 0:37123f30e8b2 579 //}
pmmccorkell 0:37123f30e8b2 580 pc.printf("log: THREAD END, horizon\r\n");
pmmccorkell 0:37123f30e8b2 581 return returnval;
pmmccorkell 0:37123f30e8b2 582 }
pmmccorkell 0:37123f30e8b2 583
pmmccorkell 0:37123f30e8b2 584 //Todo: Update for edge case around 90deg rather than 0-360.
pmmccorkell 0:37123f30e8b2 585 //Observed: BNO never actually reaches +/-90deg, flips around 84deg.
pmmccorkell 0:37123f30e8b2 586 // Arguably Quarternion may be better, however do not anticipate using more than 45deg.
pmmccorkell 0:37123f30e8b2 587 double pitchController(void){
pmmccorkell 0:37123f30e8b2 588 // call_threads_available=0;
pmmccorkell 0:37123f30e8b2 589 double speed=0;
pmmccorkell 0:37123f30e8b2 590 double desired_pitch=persistent_pitch;
pmmccorkell 0:37123f30e8b2 591 //Ensure there is a valid pitch set point before continuing
pmmccorkell 0:37123f30e8b2 592 if (desired_pitch!=-1) {
pmmccorkell 0:37123f30e8b2 593 double diff=0;
pmmccorkell 0:37123f30e8b2 594 double current_pitch=pitch;
pmmccorkell 0:37123f30e8b2 595 //Compensate for BNO having 16bit resolution per degree
pmmccorkell 0:37123f30e8b2 596 current_pitch=(current_pitch/16);
pmmccorkell 0:37123f30e8b2 597
pmmccorkell 0:37123f30e8b2 598 //Calculate how far to turn in degrees.
pmmccorkell 0:37123f30e8b2 599 diff = (desired_pitch-current_pitch);
pmmccorkell 0:37123f30e8b2 600 //Send error to PID.
pmmccorkell 0:37123f30e8b2 601 speed=pid_pitch.process(diff);
pmmccorkell 0:37123f30e8b2 602
pmmccorkell 0:37123f30e8b2 603 //Necessary to overcome ESC' 25us deadzone around 1.5ms.
pmmccorkell 0:37123f30e8b2 604 if ((fabs(speed)<min_thruster_bias) and (diff!=0)) {
pmmccorkell 0:37123f30e8b2 605 if (speed<0) speed=-1*(min_thruster_bias);
pmmccorkell 0:37123f30e8b2 606 else speed=min_thruster_bias;
pmmccorkell 0:37123f30e8b2 607 }
pmmccorkell 0:37123f30e8b2 608 //Serial log statements
pmmccorkell 0:37123f30e8b2 609 //Commented out to reduce serial lag induced at 115200 baud
pmmccorkell 0:37123f30e8b2 610 //pc.printf("log: Pcntrl, factor: %f, speed: %f\r\n",factor,speed);
pmmccorkell 0:37123f30e8b2 611 //pc.printf("log: Pcntrl, desired: %f, current: %f, diff: %f\r\n",desired_pitch,current_pitch,diff);
pmmccorkell 0:37123f30e8b2 612 }
pmmccorkell 0:37123f30e8b2 613 // call_threads_available=1;
pmmccorkell 0:37123f30e8b2 614 return (speed);
pmmccorkell 0:37123f30e8b2 615 }
pmmccorkell 0:37123f30e8b2 616
pmmccorkell 0:37123f30e8b2 617 double depthController(void){
pmmccorkell 0:37123f30e8b2 618 // call_threads_available=0;
pmmccorkell 0:37123f30e8b2 619 double speed=0;
pmmccorkell 0:37123f30e8b2 620 double desired_depth=persistent_depth;
pmmccorkell 0:37123f30e8b2 621 double current_depth=depth;
pmmccorkell 0:37123f30e8b2 622 // double diff=abs(desired_depth-current_depth);
pmmccorkell 0:37123f30e8b2 623 double diff = desired_depth-current_depth;
pmmccorkell 0:37123f30e8b2 624
pmmccorkell 0:37123f30e8b2 625 //Send the error to PID.
pmmccorkell 0:37123f30e8b2 626 speed = pid_depth.process(diff);
pmmccorkell 0:37123f30e8b2 627
pmmccorkell 0:37123f30e8b2 628 //Necessary to overcome ESC' 25us deadzone around 1.5ms.
pmmccorkell 0:37123f30e8b2 629 if ((speed<min_thruster_bias) and (diff!=0)) speed=min_thruster_bias;
pmmccorkell 0:37123f30e8b2 630
pmmccorkell 0:37123f30e8b2 631 //Serial log statements
pmmccorkell 0:37123f30e8b2 632 //Commented out to reduce serial lag induced at 115200 baud
pmmccorkell 0:37123f30e8b2 633 //pc.printf("log: Dcntrl, factor: %f, speed: %f\r\n",factor,speed);
pmmccorkell 0:37123f30e8b2 634 //pc.printf("log: Dcntrl, desired: %f, current: %f, diff: %f\r\n",desired_depth,current_depth,diff);
pmmccorkell 0:37123f30e8b2 635
pmmccorkell 0:37123f30e8b2 636 // call_threads_available=1;
pmmccorkell 0:37123f30e8b2 637 return speed;
pmmccorkell 0:37123f30e8b2 638 }
pmmccorkell 0:37123f30e8b2 639
pmmccorkell 0:37123f30e8b2 640 //Function to handle vertical motor logic.
pmmccorkell 0:37123f30e8b2 641 void el_thruster_logic() {
pmmccorkell 0:37123f30e8b2 642 if (manual_mode==0) {
pmmccorkell 0:37123f30e8b2 643 //Internal debugging and time measurements.
pmmccorkell 0:37123f30e8b2 644 //function_timer=1;
pmmccorkell 0:37123f30e8b2 645 // logic_available=0;
pmmccorkell 0:37123f30e8b2 646
pmmccorkell 0:37123f30e8b2 647 //Instantiate controllers at 0.
pmmccorkell 0:37123f30e8b2 648 double depth_speed=0;
pmmccorkell 0:37123f30e8b2 649 double pitch_speed=0;
pmmccorkell 0:37123f30e8b2 650 double aft_speed=0;
pmmccorkell 0:37123f30e8b2 651 double fore_speed=0;
pmmccorkell 0:37123f30e8b2 652
pmmccorkell 0:37123f30e8b2 653 //Call Depth and/or Pitch controllers only if they have a valid set point.
pmmccorkell 0:37123f30e8b2 654 if (persistent_depth!=-1) {
pmmccorkell 0:37123f30e8b2 655 depth_speed=depthController();
pmmccorkell 0:37123f30e8b2 656 }
pmmccorkell 0:37123f30e8b2 657 if (persistent_pitch!=-1) {
pmmccorkell 0:37123f30e8b2 658 pitch_speed=pitchController();
pmmccorkell 0:37123f30e8b2 659 }
pmmccorkell 0:37123f30e8b2 660
pmmccorkell 0:37123f30e8b2 661 //If both Controllers are active, it's easy to saturate the ESCs at +/-400ms.
pmmccorkell 0:37123f30e8b2 662 //Arbitrarily chose 75% for creating superposition of Depth and Pitch, this could be refined.
pmmccorkell 0:37123f30e8b2 663 if ((persistent_pitch!=-1) and (persistent_depth!=-1)) {
pmmccorkell 0:37123f30e8b2 664 depth_speed=depth_speed*.75;
pmmccorkell 0:37123f30e8b2 665 pitch_speed=pitch_speed*.75;
pmmccorkell 0:37123f30e8b2 666 }
pmmccorkell 0:37123f30e8b2 667
pmmccorkell 0:37123f30e8b2 668 //Form superposition independently for Aft and Fore ESCs
pmmccorkell 0:37123f30e8b2 669 // from depth and pitch controllers.
pmmccorkell 0:37123f30e8b2 670 aft_speed=(depth_speed - pitch_speed);
pmmccorkell 0:37123f30e8b2 671 fore_speed=(depth_speed + pitch_speed);
pmmccorkell 0:37123f30e8b2 672
pmmccorkell 0:37123f30e8b2 673 //Ensure values don't exceed ESC operational range: ([-400,400]ms + 1.5ms).
pmmccorkell 0:37123f30e8b2 674 if (aft_speed<(-1*esc_range)) aft_speed=(-1*esc_range);
pmmccorkell 0:37123f30e8b2 675 if (fore_speed<(-1*esc_range)) fore_speed=(-1*esc_range);
pmmccorkell 0:37123f30e8b2 676 if (aft_speed>esc_range) aft_speed=esc_range;
pmmccorkell 0:37123f30e8b2 677 if (fore_speed>esc_range) fore_speed=esc_range;
pmmccorkell 0:37123f30e8b2 678
pmmccorkell 0:37123f30e8b2 679 //Only update PWM values if they're appreciably different from current state.
pmmccorkell 0:37123f30e8b2 680 //Log print statements commented out to reduce serial lag at 115200 baud rate.
pmmccorkell 0:37123f30e8b2 681 double current_aft_pw = aft_thrust.get_pw();
pmmccorkell 0:37123f30e8b2 682 double current_fore_pw = fore_thrust.get_pw();
pmmccorkell 0:37123f30e8b2 683 double compare_aft_pw=fabs((1.5+aft_speed)-current_aft_pw);
pmmccorkell 0:37123f30e8b2 684 double compare_fore_pw=fabs((1.5+fore_speed)-current_fore_pw);
pmmccorkell 0:37123f30e8b2 685 if (compare_aft_pw > pw_tolerance) {
pmmccorkell 0:37123f30e8b2 686 aft_thrust.set_speed(aft_speed);
pmmccorkell 0:37123f30e8b2 687 //pc.printf("log: aft %f\r\n",aft_speed);
pmmccorkell 0:37123f30e8b2 688 }
pmmccorkell 0:37123f30e8b2 689 if (compare_fore_pw > pw_tolerance) {
pmmccorkell 0:37123f30e8b2 690 fore_thrust.set_speed(fore_speed);
pmmccorkell 0:37123f30e8b2 691 //pc.printf("log: fore %f\r\n",fore_speed);
pmmccorkell 0:37123f30e8b2 692 }
pmmccorkell 0:37123f30e8b2 693 //Internal debugging and time measurements.
pmmccorkell 0:37123f30e8b2 694 // logic_available=1;
pmmccorkell 0:37123f30e8b2 695 //function_timer=0;
pmmccorkell 0:37123f30e8b2 696 }
pmmccorkell 0:37123f30e8b2 697 }
pmmccorkell 0:37123f30e8b2 698
pmmccorkell 0:37123f30e8b2 699 //Function to go to set heading.
pmmccorkell 0:37123f30e8b2 700 //Timed with DigitalOut on Oscope.
pmmccorkell 0:37123f30e8b2 701 //With no heading set, 28.6us.
pmmccorkell 0:37123f30e8b2 702 //With heading calculations, ~32.8us
pmmccorkell 0:37123f30e8b2 703 //With logging added, ~1.278ms -> serial print statements induce significant delays
pmmccorkell 0:37123f30e8b2 704 double headingController() {
pmmccorkell 0:37123f30e8b2 705 // call_threads_available=0;
pmmccorkell 0:37123f30e8b2 706 double diff=0;
pmmccorkell 0:37123f30e8b2 707 double speed=0;
pmmccorkell 0:37123f30e8b2 708 double dir=1;
pmmccorkell 0:37123f30e8b2 709 double desired_heading=persistent_heading;
pmmccorkell 0:37123f30e8b2 710 double current_heading=heading;
pmmccorkell 0:37123f30e8b2 711 current_heading=(current_heading/16);
pmmccorkell 0:37123f30e8b2 712 if (desired_heading!=-1) {
pmmccorkell 0:37123f30e8b2 713 //Calculate how far to turn in degrees.
pmmccorkell 0:37123f30e8b2 714 diff = abs(desired_heading-current_heading);
pmmccorkell 0:37123f30e8b2 715 //pc.printf("log: diff before if: %f\r\n",diff);
pmmccorkell 0:37123f30e8b2 716
pmmccorkell 0:37123f30e8b2 717 //Correct for 360-0 edge cases if 'diff'erence is greater than 180.
pmmccorkell 0:37123f30e8b2 718 //Change direction and recalculate for accurate 'tolerance' comparison.
pmmccorkell 0:37123f30e8b2 719 if (diff>180.0) {
pmmccorkell 0:37123f30e8b2 720 //pc.printf("log: diff>180: %f\r\n",diff);
pmmccorkell 0:37123f30e8b2 721 if (desired_heading>180) {
pmmccorkell 0:37123f30e8b2 722 //dir=(-1*dir);
pmmccorkell 0:37123f30e8b2 723 current_heading=current_heading+180;
pmmccorkell 0:37123f30e8b2 724 desired_heading=desired_heading-180;
pmmccorkell 0:37123f30e8b2 725 diff=current_heading-desired_heading;
pmmccorkell 0:37123f30e8b2 726 pc.printf("log: desired>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff);
pmmccorkell 0:37123f30e8b2 727 }
pmmccorkell 0:37123f30e8b2 728 if (current_heading>180) {
pmmccorkell 0:37123f30e8b2 729 dir=(-1*dir);
pmmccorkell 0:37123f30e8b2 730 current_heading=current_heading-180;
pmmccorkell 0:37123f30e8b2 731 desired_heading=desired_heading+180;
pmmccorkell 0:37123f30e8b2 732 diff=desired_heading-current_heading;
pmmccorkell 0:37123f30e8b2 733 }
pmmccorkell 0:37123f30e8b2 734 }
pmmccorkell 0:37123f30e8b2 735
pmmccorkell 0:37123f30e8b2 736 speed = pid_heading.process(diff);
pmmccorkell 0:37123f30e8b2 737
pmmccorkell 0:37123f30e8b2 738 //Necessary to overcome 25us deadzone around 1.5ms
pmmccorkell 0:37123f30e8b2 739 if ((abs(speed)<min_thruster_bias) and (diff!=0)) {
pmmccorkell 0:37123f30e8b2 740 if (speed<0) speed=(-1*min_thruster_bias);
pmmccorkell 0:37123f30e8b2 741 else speed=min_thruster_bias;
pmmccorkell 0:37123f30e8b2 742 }
pmmccorkell 0:37123f30e8b2 743 //Serial log statements
pmmccorkell 0:37123f30e8b2 744 //Commented out to reduce serial lag induced at 115200 baud
pmmccorkell 0:37123f30e8b2 745 // pc.printf("log: Hcntrl, factor: %f, speed: %f\r\n",factor,speed);
pmmccorkell 0:37123f30e8b2 746 // pc.printf("log: Hcntrl, desired: %f, current: %f, diff: %f\r\n",desired_heading,current_heading,diff);
pmmccorkell 0:37123f30e8b2 747 }
pmmccorkell 0:37123f30e8b2 748 // call_threads_available=1;
pmmccorkell 0:37123f30e8b2 749 return (speed*dir);
pmmccorkell 0:37123f30e8b2 750 }
pmmccorkell 0:37123f30e8b2 751
pmmccorkell 0:37123f30e8b2 752 //Controller to maintain the scalar component of velocity vector.
pmmccorkell 0:37123f30e8b2 753 double speedController() {
pmmccorkell 0:37123f30e8b2 754 // call_threads_available=0;
pmmccorkell 0:37123f30e8b2 755 double desired_speed=persistent_speed;
pmmccorkell 0:37123f30e8b2 756 desired_speed=(desired_speed/1000); //convert int us to ms
pmmccorkell 0:37123f30e8b2 757
pmmccorkell 0:37123f30e8b2 758 // ENTER PID CALCS HERE //
pmmccorkell 0:37123f30e8b2 759
pmmccorkell 0:37123f30e8b2 760 // call_threads_available=1;
pmmccorkell 0:37123f30e8b2 761 return (desired_speed);
pmmccorkell 0:37123f30e8b2 762 }
pmmccorkell 0:37123f30e8b2 763
pmmccorkell 0:37123f30e8b2 764 // Make superposition of all Controllers accessing thrusters acting on Az plane.
pmmccorkell 0:37123f30e8b2 765 // Only function that shall write to Az plane thrusters.
pmmccorkell 0:37123f30e8b2 766 // This will also deprecate call_threads() function.
pmmccorkell 0:37123f30e8b2 767 void az_thruster_logic() {
pmmccorkell 0:37123f30e8b2 768 if (manual_mode==0) {
pmmccorkell 0:37123f30e8b2 769 // function_timer3=1;
pmmccorkell 0:37123f30e8b2 770 // logic_available=0;
pmmccorkell 0:37123f30e8b2 771 double heading_speed=0;
pmmccorkell 0:37123f30e8b2 772 double sp_speed=0;
pmmccorkell 0:37123f30e8b2 773 double starboard_speed=0;
pmmccorkell 0:37123f30e8b2 774 double port_speed=0;
pmmccorkell 0:37123f30e8b2 775 if (persistent_speed==-1) sp_speed=0;
pmmccorkell 0:37123f30e8b2 776 if (persistent_heading==-1) heading_speed=0;
pmmccorkell 0:37123f30e8b2 777 if (persistent_heading!=-1) {
pmmccorkell 0:37123f30e8b2 778 heading_speed=headingController();
pmmccorkell 0:37123f30e8b2 779 }
pmmccorkell 0:37123f30e8b2 780 if (persistent_speed!=-1) {
pmmccorkell 0:37123f30e8b2 781 sp_speed=speedController();
pmmccorkell 0:37123f30e8b2 782 }
pmmccorkell 0:37123f30e8b2 783 /*
pmmccorkell 0:37123f30e8b2 784 if ((persistent_heading!=-1) and (persistent_speed!=-1)) {
pmmccorkell 0:37123f30e8b2 785 heading_speed=heading_speed*.75;
pmmccorkell 0:37123f30e8b2 786 sp_speed=sp_speed*.75;
pmmccorkell 0:37123f30e8b2 787 }
pmmccorkell 0:37123f30e8b2 788 */
pmmccorkell 0:37123f30e8b2 789
pmmccorkell 0:37123f30e8b2 790 //Create Superposition of Heading and Speed
pmmccorkell 0:37123f30e8b2 791 //May need to divide these by 2 ?
pmmccorkell 0:37123f30e8b2 792 starboard_speed=(sp_speed - heading_speed);
pmmccorkell 0:37123f30e8b2 793 port_speed=(sp_speed + heading_speed);
pmmccorkell 0:37123f30e8b2 794
pmmccorkell 0:37123f30e8b2 795 //Error checking to ensure PWM doesn't escape ESC parameters
pmmccorkell 0:37123f30e8b2 796 if (port_speed<(-1*esc_range)) port_speed=(-1*esc_range);
pmmccorkell 0:37123f30e8b2 797 if (starboard_speed<(-1*esc_range)) starboard_speed=(-1*esc_range);
pmmccorkell 0:37123f30e8b2 798 if (port_speed>esc_range) port_speed=esc_range;
pmmccorkell 0:37123f30e8b2 799 if (starboard_speed>esc_range) starboard_speed=esc_range;
pmmccorkell 0:37123f30e8b2 800
pmmccorkell 0:37123f30e8b2 801 //Only write PWM if PW is appreciably different
pmmccorkell 0:37123f30e8b2 802 double current_starboard_pw = starboard_thrust.get_pw();
pmmccorkell 0:37123f30e8b2 803 double current_port_pw = port_thrust.get_pw();
pmmccorkell 0:37123f30e8b2 804 double compare_port_pw=fabs((1.5+port_speed)-current_port_pw);
pmmccorkell 0:37123f30e8b2 805 double compare_starboard_pw=fabs((1.5+starboard_speed)-current_starboard_pw);
pmmccorkell 0:37123f30e8b2 806 if (compare_port_pw > pw_tolerance) {
pmmccorkell 0:37123f30e8b2 807 port_thrust.set_speed(port_speed);
pmmccorkell 0:37123f30e8b2 808 //pc.printf("log: port %f\r\n",port_speed);
pmmccorkell 0:37123f30e8b2 809 }
pmmccorkell 0:37123f30e8b2 810 if (compare_starboard_pw > pw_tolerance) {
pmmccorkell 0:37123f30e8b2 811 starboard_thrust.set_speed(starboard_speed);
pmmccorkell 0:37123f30e8b2 812 //pc.printf("log: stbd %f\r\n",starboard_speed);
pmmccorkell 0:37123f30e8b2 813 }
pmmccorkell 0:37123f30e8b2 814 // logic_available=1;
pmmccorkell 0:37123f30e8b2 815 // function_timer3=0;
pmmccorkell 0:37123f30e8b2 816 }
pmmccorkell 0:37123f30e8b2 817 }
pmmccorkell 0:37123f30e8b2 818
pmmccorkell 0:37123f30e8b2 819
pmmccorkell 0:37123f30e8b2 820 //Function to change BNO position
pmmccorkell 0:37123f30e8b2 821 void switch_pos(int position) {
pmmccorkell 0:37123f30e8b2 822 uint16_t ready_mask=0xfff8;
pmmccorkell 0:37123f30e8b2 823 if (position>=0 and position<8) {
pmmccorkell 0:37123f30e8b2 824 switch (position) {
pmmccorkell 0:37123f30e8b2 825 case 1:
pmmccorkell 0:37123f30e8b2 826 imu.set_mounting_position(MT_P1);
pmmccorkell 0:37123f30e8b2 827 ready_data=((ready_data & ready_mask)+0x001);
pmmccorkell 0:37123f30e8b2 828 break;
pmmccorkell 0:37123f30e8b2 829 case 2:
pmmccorkell 0:37123f30e8b2 830 imu.set_mounting_position(MT_P2);
pmmccorkell 0:37123f30e8b2 831 ready_data=((ready_data & ready_mask)+0x002);
pmmccorkell 0:37123f30e8b2 832 break;
pmmccorkell 0:37123f30e8b2 833 case 3:
pmmccorkell 0:37123f30e8b2 834 imu.set_mounting_position(MT_P3);
pmmccorkell 0:37123f30e8b2 835 ready_data=((ready_data & ready_mask)+0x003);
pmmccorkell 0:37123f30e8b2 836 break;
pmmccorkell 0:37123f30e8b2 837 case 4:
pmmccorkell 0:37123f30e8b2 838 imu.set_mounting_position(MT_P4);
pmmccorkell 0:37123f30e8b2 839 ready_data=((ready_data & ready_mask)+0x004);
pmmccorkell 0:37123f30e8b2 840 break;
pmmccorkell 0:37123f30e8b2 841 case 5:
pmmccorkell 0:37123f30e8b2 842 imu.set_mounting_position(MT_P5);
pmmccorkell 0:37123f30e8b2 843 ready_data=((ready_data & ready_mask)+0x005);
pmmccorkell 0:37123f30e8b2 844 break;
pmmccorkell 0:37123f30e8b2 845 case 6:
pmmccorkell 0:37123f30e8b2 846 imu.set_mounting_position(MT_P6);
pmmccorkell 0:37123f30e8b2 847 ready_data=((ready_data & ready_mask)+0x006);
pmmccorkell 0:37123f30e8b2 848 break;
pmmccorkell 0:37123f30e8b2 849 case 7:
pmmccorkell 0:37123f30e8b2 850 imu.set_mounting_position(MT_P7);
pmmccorkell 0:37123f30e8b2 851 ready_data=((ready_data & ready_mask)+0x007);
pmmccorkell 0:37123f30e8b2 852 break;
pmmccorkell 0:37123f30e8b2 853 case 0:
pmmccorkell 0:37123f30e8b2 854 default:
pmmccorkell 0:37123f30e8b2 855 imu.set_mounting_position(MT_P0);
pmmccorkell 0:37123f30e8b2 856 ready_data=((ready_data & ready_mask));
pmmccorkell 0:37123f30e8b2 857 break;
pmmccorkell 0:37123f30e8b2 858 }
pmmccorkell 0:37123f30e8b2 859 }
pmmccorkell 0:37123f30e8b2 860 }
pmmccorkell 0:37123f30e8b2 861
pmmccorkell 0:37123f30e8b2 862 //Mostly deprecated.
pmmccorkell 0:37123f30e8b2 863 //Manual direction function allows manual control of AUV.
pmmccorkell 0:37123f30e8b2 864 void manual_direction(int direction,int mode) {
pmmccorkell 0:37123f30e8b2 865 // double speed_rate=0.25; //25% speed of global max.
pmmccorkell 0:37123f30e8b2 866 //double speed=(esc_range*speed_rate); //This rate% of global% set in globals. Expected (70-90%).
pmmccorkell 0:37123f30e8b2 867 double speed=min_thruster_bias;
pmmccorkell 0:37123f30e8b2 868 double dir;
pmmccorkell 0:37123f30e8b2 869 if (direction==1) dir = 1.0;
pmmccorkell 0:37123f30e8b2 870 else if (direction==2) dir=(-1);
pmmccorkell 0:37123f30e8b2 871 else mode=0.0;
pmmccorkell 0:37123f30e8b2 872 if (mode==0) {
pmmccorkell 0:37123f30e8b2 873 //stop az Thrusters
pmmccorkell 0:37123f30e8b2 874 call_threads(0);
pmmccorkell 0:37123f30e8b2 875 //stop el Thrusters
pmmccorkell 0:37123f30e8b2 876 call_threads(99);
pmmccorkell 0:37123f30e8b2 877 }
pmmccorkell 0:37123f30e8b2 878 else {
pmmccorkell 0:37123f30e8b2 879 call_threads(mode,dir,speed);
pmmccorkell 0:37123f30e8b2 880 }
pmmccorkell 0:37123f30e8b2 881 }
pmmccorkell 0:37123f30e8b2 882
pmmccorkell 0:37123f30e8b2 883 //Mostly deprecated.
pmmccorkell 0:37123f30e8b2 884 //Tests +/- direction of heading, speed, depth, and pitch for demonstration.
pmmccorkell 0:37123f30e8b2 885 void test_direction(int mode) {
pmmccorkell 0:37123f30e8b2 886 //pc.printf("test_dir thread, mode:%i\r\n",mode);
pmmccorkell 0:37123f30e8b2 887 int count=0;
pmmccorkell 0:37123f30e8b2 888 int time=450;
pmmccorkell 0:37123f30e8b2 889 int length=0;
pmmccorkell 0:37123f30e8b2 890 manual_direction(1,mode);
pmmccorkell 0:37123f30e8b2 891 while(count<time) {
pmmccorkell 0:37123f30e8b2 892 int check=0;
pmmccorkell 0:37123f30e8b2 893 if (count==200) {
pmmccorkell 0:37123f30e8b2 894 //pc.printf("stop in middle of test/r/n");
pmmccorkell 0:37123f30e8b2 895 if ((mode==1) or (mode==2)) {
pmmccorkell 0:37123f30e8b2 896 call_threads(0);
pmmccorkell 0:37123f30e8b2 897 }
pmmccorkell 0:37123f30e8b2 898 if ((mode==3) or (mode==4)) {
pmmccorkell 0:37123f30e8b2 899 call_threads(99);
pmmccorkell 0:37123f30e8b2 900 }
pmmccorkell 0:37123f30e8b2 901 }
pmmccorkell 0:37123f30e8b2 902 if (count==250) {
pmmccorkell 0:37123f30e8b2 903 //pc.printf("reverse direction\r\n");
pmmccorkell 0:37123f30e8b2 904 if (event_horizon_flag==0) manual_direction(2,mode);
pmmccorkell 0:37123f30e8b2 905 }
pmmccorkell 0:37123f30e8b2 906 //pc.printf("log: cnt: %i\r\n",count);
pmmccorkell 0:37123f30e8b2 907 if (pc.readable()) {
pmmccorkell 0:37123f30e8b2 908 length=read_serial();
pmmccorkell 0:37123f30e8b2 909 if (length==4) {
pmmccorkell 0:37123f30e8b2 910 check=look_horizon();
pmmccorkell 0:37123f30e8b2 911 }
pmmccorkell 0:37123f30e8b2 912 }
pmmccorkell 0:37123f30e8b2 913 if (check==1) count=time;
pmmccorkell 0:37123f30e8b2 914 else {
pmmccorkell 0:37123f30e8b2 915 count++;
pmmccorkell 0:37123f30e8b2 916 wait_ms(wait_main);
pmmccorkell 0:37123f30e8b2 917 az_data();
pmmccorkell 0:37123f30e8b2 918 }
pmmccorkell 0:37123f30e8b2 919 }
pmmccorkell 0:37123f30e8b2 920 //stop az thrusters
pmmccorkell 0:37123f30e8b2 921 if ((mode==1) or (mode==2)) {
pmmccorkell 0:37123f30e8b2 922 call_threads(0);
pmmccorkell 0:37123f30e8b2 923 }
pmmccorkell 0:37123f30e8b2 924 //stop el thrusters
pmmccorkell 0:37123f30e8b2 925 if ((mode==3) or (mode==4)) {
pmmccorkell 0:37123f30e8b2 926 call_threads(99);
pmmccorkell 0:37123f30e8b2 927 }
pmmccorkell 0:37123f30e8b2 928 }
pmmccorkell 0:37123f30e8b2 929
pmmccorkell 0:37123f30e8b2 930 //Function for manual up/down, left/right controls.
pmmccorkell 0:37123f30e8b2 931 //Takes current measurement of relevant sensor and adds/subtracts the "resolution" times "magnitude"
pmmccorkell 0:37123f30e8b2 932 // to new set point, incrementally driving vehicle.
pmmccorkell 0:37123f30e8b2 933 // ie current heading 10deg, function(3,-1), new heading set point of 10+(-1*3) = 7deg
pmmccorkell 0:37123f30e8b2 934 void increment_persistent(int select, int magnitude) {
pmmccorkell 0:37123f30e8b2 935 int pitch_resolution=3; //degrees
pmmccorkell 0:37123f30e8b2 936 int depth_resolution=3; //cm
pmmccorkell 0:37123f30e8b2 937 int heading_resolution=3; //degrees
pmmccorkell 0:37123f30e8b2 938 int speed_resolution=27; //us
pmmccorkell 0:37123f30e8b2 939 switch (select) {
pmmccorkell 0:37123f30e8b2 940 //pitch
pmmccorkell 0:37123f30e8b2 941 case 1:
pmmccorkell 0:37123f30e8b2 942 persistent_pitch=((pitch/16)+(magnitude*pitch_resolution));
pmmccorkell 0:37123f30e8b2 943 pid_pitch.clear();
pmmccorkell 0:37123f30e8b2 944 break;
pmmccorkell 0:37123f30e8b2 945
pmmccorkell 0:37123f30e8b2 946 //depth
pmmccorkell 0:37123f30e8b2 947 case 2:
pmmccorkell 0:37123f30e8b2 948 persistent_depth=(depth+(magnitude*depth_resolution));
pmmccorkell 0:37123f30e8b2 949 pid_depth.clear();
pmmccorkell 0:37123f30e8b2 950 break;
pmmccorkell 0:37123f30e8b2 951
pmmccorkell 0:37123f30e8b2 952 //heading
pmmccorkell 0:37123f30e8b2 953 case 3:
pmmccorkell 0:37123f30e8b2 954 persistent_heading=((heading/16)+(magnitude*heading_resolution));
pmmccorkell 0:37123f30e8b2 955 pid_heading.clear();
pmmccorkell 0:37123f30e8b2 956 break;
pmmccorkell 0:37123f30e8b2 957
pmmccorkell 0:37123f30e8b2 958 //speed
pmmccorkell 0:37123f30e8b2 959 case 4:
pmmccorkell 0:37123f30e8b2 960 persistent_speed=((1000*starboard_thrust.get_speed())+(magnitude*speed_resolution));
pmmccorkell 0:37123f30e8b2 961 // pid_speed.clear();
pmmccorkell 0:37123f30e8b2 962 break;
pmmccorkell 0:37123f30e8b2 963 }
pmmccorkell 0:37123f30e8b2 964 }
pmmccorkell 0:37123f30e8b2 965
pmmccorkell 0:37123f30e8b2 966 //Function intakes serial commands.
pmmccorkell 0:37123f30e8b2 967 void commands() {
pmmccorkell 0:37123f30e8b2 968 //pc.printf("log: commands called\r\n");
pmmccorkell 0:37123f30e8b2 969 int length=0;
pmmccorkell 0:37123f30e8b2 970 length=read_serial();
pmmccorkell 0:37123f30e8b2 971 if (length==4) {
pmmccorkell 0:37123f30e8b2 972 look_horizon();
pmmccorkell 0:37123f30e8b2 973 }
pmmccorkell 0:37123f30e8b2 974 if (length==7) {
pmmccorkell 0:37123f30e8b2 975 char input[10];
pmmccorkell 0:37123f30e8b2 976 for (int i=0; i<10; i++) {
pmmccorkell 0:37123f30e8b2 977 input[i]=readline[i];
pmmccorkell 0:37123f30e8b2 978 //pc.printf("Command thread: read %c at %i\r\n",readline[i],i);
pmmccorkell 0:37123f30e8b2 979 }
pmmccorkell 0:37123f30e8b2 980 //check_pos char array used to filter Position commands.
pmmccorkell 0:37123f30e8b2 981 char check_pos[5]="pos:";
pmmccorkell 0:37123f30e8b2 982 //check_hea char array used to filter Go to Heading commands.
pmmccorkell 0:37123f30e8b2 983 char check_hea[5]="hea:";
pmmccorkell 0:37123f30e8b2 984 //check_dep char array used to filter Depth commands.
pmmccorkell 0:37123f30e8b2 985 char check_dep[5]="dep:";
pmmccorkell 0:37123f30e8b2 986 char check_zer[5]="zer:";
pmmccorkell 0:37123f30e8b2 987 char check_pit[5]="pit:";
pmmccorkell 0:37123f30e8b2 988 char check_tst[5]="tst:";
pmmccorkell 0:37123f30e8b2 989 char check_sto[5]="sto:";
pmmccorkell 0:37123f30e8b2 990 char check_res[5]="res:";
pmmccorkell 0:37123f30e8b2 991 char check_vel[5]="vel:";
pmmccorkell 0:37123f30e8b2 992
pmmccorkell 0:37123f30e8b2 993 //While loop reads Serial input into input buffer.
pmmccorkell 0:37123f30e8b2 994
pmmccorkell 0:37123f30e8b2 995 //Only continue if input buffer has 7 entries, otherwise ignore input buffer.
pmmccorkell 0:37123f30e8b2 996 //All commands from RasPi shall come in a format of 7 characters "abc:xyz"
pmmccorkell 0:37123f30e8b2 997 // where 'abc' is an identifying string and 'xyz' is some data/information.
pmmccorkell 0:37123f30e8b2 998 // if (i==7) {
pmmccorkell 0:37123f30e8b2 999 //Instantiate counters that will be used to verify known commands.
pmmccorkell 0:37123f30e8b2 1000 int verified_pos=0;
pmmccorkell 0:37123f30e8b2 1001 int verified_hea=0;
pmmccorkell 0:37123f30e8b2 1002 int verified_dep=0;
pmmccorkell 0:37123f30e8b2 1003 int verified_zer=0;
pmmccorkell 0:37123f30e8b2 1004 int verified_pit=0;
pmmccorkell 0:37123f30e8b2 1005 int verified_tst=0;
pmmccorkell 0:37123f30e8b2 1006 int verified_sto=0;
pmmccorkell 0:37123f30e8b2 1007 int verified_res=0;
pmmccorkell 0:37123f30e8b2 1008 int verified_vel=0;
pmmccorkell 0:37123f30e8b2 1009 //While loop checks first 4 characters of input buffer and attempts to match
pmmccorkell 0:37123f30e8b2 1010 // against known commands.
pmmccorkell 0:37123f30e8b2 1011 for (int q=0; q<3; q++) {
pmmccorkell 0:37123f30e8b2 1012 //Increment verified_pos if a match is found between Serial input buffer
pmmccorkell 0:37123f30e8b2 1013 // and Position command format.
pmmccorkell 0:37123f30e8b2 1014 if (input[q]==check_pos[q]) {
pmmccorkell 0:37123f30e8b2 1015 verified_pos++;
pmmccorkell 0:37123f30e8b2 1016 pc.printf("pos %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1017 }
pmmccorkell 0:37123f30e8b2 1018 //Increment verified_hea if a match is found between Serial input buffer
pmmccorkell 0:37123f30e8b2 1019 // and Heading command format.
pmmccorkell 0:37123f30e8b2 1020 if (input[q]==check_hea[q]) {
pmmccorkell 0:37123f30e8b2 1021 //pc.printf("hea %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1022 verified_hea++;
pmmccorkell 0:37123f30e8b2 1023 }
pmmccorkell 0:37123f30e8b2 1024 if (input[q]==check_dep[q]) {
pmmccorkell 0:37123f30e8b2 1025 //pc.printf("dep %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1026 verified_dep++;
pmmccorkell 0:37123f30e8b2 1027 }
pmmccorkell 0:37123f30e8b2 1028 if (input[q]==check_zer[q]) {
pmmccorkell 0:37123f30e8b2 1029 //pc.printf("zer %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1030 verified_zer++;
pmmccorkell 0:37123f30e8b2 1031 }
pmmccorkell 0:37123f30e8b2 1032 if (input[q]==check_pit[q]) {
pmmccorkell 0:37123f30e8b2 1033 //pc.printf("pit %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1034 verified_pit++;
pmmccorkell 0:37123f30e8b2 1035 }
pmmccorkell 0:37123f30e8b2 1036 if (input[q]==check_tst[q]) {
pmmccorkell 0:37123f30e8b2 1037 //pc.printf("tst %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1038 verified_tst++;
pmmccorkell 0:37123f30e8b2 1039 }
pmmccorkell 0:37123f30e8b2 1040 if (input[q]==check_sto[q]) {
pmmccorkell 0:37123f30e8b2 1041 //pc.printf("sto %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1042 verified_sto++;
pmmccorkell 0:37123f30e8b2 1043 }
pmmccorkell 0:37123f30e8b2 1044 if (input[q]==check_res[q]) {
pmmccorkell 0:37123f30e8b2 1045 //pc.printf("res %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1046 verified_res++;
pmmccorkell 0:37123f30e8b2 1047 }
pmmccorkell 0:37123f30e8b2 1048 if (input[q]==check_vel[q]) {
pmmccorkell 0:37123f30e8b2 1049 verified_vel++;
pmmccorkell 0:37123f30e8b2 1050 //pc.printf("vel %c at %i\r\n",input[q],q);
pmmccorkell 0:37123f30e8b2 1051 }
pmmccorkell 0:37123f30e8b2 1052 }
pmmccorkell 0:37123f30e8b2 1053
pmmccorkell 0:37123f30e8b2 1054 //If first 4 characters from Serial input buffer match Position command format,
pmmccorkell 0:37123f30e8b2 1055 // execute "switch_pos" function.
pmmccorkell 0:37123f30e8b2 1056 if (verified_pos==3) {
pmmccorkell 0:37123f30e8b2 1057 int change=(input[6]-'0');
pmmccorkell 0:37123f30e8b2 1058 switch_pos(change);
pmmccorkell 0:37123f30e8b2 1059 }
pmmccorkell 0:37123f30e8b2 1060 if (verified_sto==3) {
pmmccorkell 0:37123f30e8b2 1061 //pc.printf("log: stop command received\r\n");
pmmccorkell 0:37123f30e8b2 1062 stop_all_persistent();
pmmccorkell 0:37123f30e8b2 1063 //pc.printf("log: stop command executed\r\n");
pmmccorkell 0:37123f30e8b2 1064 }
pmmccorkell 0:37123f30e8b2 1065 //If first 4 characters from Serial input buffer match Heading command format,
pmmccorkell 0:37123f30e8b2 1066 // execute "motors" function.
pmmccorkell 0:37123f30e8b2 1067 if (verified_hea==3) {
pmmccorkell 0:37123f30e8b2 1068 //Correct for ascii '0', and reform 3digit decimal number
pmmccorkell 0:37123f30e8b2 1069 int hea100=(input[4]-'0');
pmmccorkell 0:37123f30e8b2 1070 int hea10=(input[5]-'0');
pmmccorkell 0:37123f30e8b2 1071 int hea1=(input[6]-'0');
pmmccorkell 0:37123f30e8b2 1072 int hea=(hea100*100)+(hea10*10)+(hea1*1);
pmmccorkell 0:37123f30e8b2 1073 pc.printf("log: heading rx: %i\r\n",hea);
pmmccorkell 0:37123f30e8b2 1074 if (hea==999) {
pmmccorkell 0:37123f30e8b2 1075 persistent_heading=-1;
pmmccorkell 0:37123f30e8b2 1076 pid_heading.clear();
pmmccorkell 0:37123f30e8b2 1077 }
pmmccorkell 0:37123f30e8b2 1078 if ((hea>=831) and (hea<=837)) {
pmmccorkell 0:37123f30e8b2 1079 increment_persistent(hea10,(hea1-4));
pmmccorkell 0:37123f30e8b2 1080 }
pmmccorkell 0:37123f30e8b2 1081 if ((hea>=0) and (hea<=360)) {
pmmccorkell 0:37123f30e8b2 1082 pid_heading.clear();
pmmccorkell 0:37123f30e8b2 1083 if (event_horizon_flag==0) persistent_heading=hea;
pmmccorkell 0:37123f30e8b2 1084 }
pmmccorkell 0:37123f30e8b2 1085 }
pmmccorkell 0:37123f30e8b2 1086 //If first 4 characters from Serial input buffer match Depth command format,
pmmccorkell 0:37123f30e8b2 1087 // execute "depth" function.
pmmccorkell 0:37123f30e8b2 1088 if (verified_dep==3) {
pmmccorkell 0:37123f30e8b2 1089 //Correct for ascii '0', and reform 3digit decimal number
pmmccorkell 0:37123f30e8b2 1090 int dep100=(input[4]-'0');
pmmccorkell 0:37123f30e8b2 1091 int dep10=(input[5]-'0');
pmmccorkell 0:37123f30e8b2 1092 int dep1=(input[6]-'0');
pmmccorkell 0:37123f30e8b2 1093 int dep=(dep100*100)+(dep10*10)+(dep1*1);
pmmccorkell 0:37123f30e8b2 1094 pc.printf("log: depth rx: %i\r\n",dep);
pmmccorkell 0:37123f30e8b2 1095 if (dep==999) {
pmmccorkell 0:37123f30e8b2 1096 persistent_depth=-1;
pmmccorkell 0:37123f30e8b2 1097 pid_depth.clear();
pmmccorkell 0:37123f30e8b2 1098 }
pmmccorkell 0:37123f30e8b2 1099 if ((dep>=821) and (dep<=827)) {
pmmccorkell 0:37123f30e8b2 1100 increment_persistent(dep10,(dep1-4));
pmmccorkell 0:37123f30e8b2 1101 }
pmmccorkell 0:37123f30e8b2 1102 if (dep<=820) {
pmmccorkell 0:37123f30e8b2 1103 pid_depth.clear();
pmmccorkell 0:37123f30e8b2 1104 if (event_horizon_flag==0) persistent_depth=dep;
pmmccorkell 0:37123f30e8b2 1105 }
pmmccorkell 0:37123f30e8b2 1106 }
pmmccorkell 0:37123f30e8b2 1107 if (verified_pit==3) {
pmmccorkell 0:37123f30e8b2 1108 //Correct for ascii '0', and reform 3digit decimal number
pmmccorkell 0:37123f30e8b2 1109 int pit100=(input[4]-'0');
pmmccorkell 0:37123f30e8b2 1110 int pit10=(input[5]-'0');
pmmccorkell 0:37123f30e8b2 1111 int pit1=(input[6]-'0');
pmmccorkell 0:37123f30e8b2 1112 int pit=(pit100*100)+(pit10*10)+(pit1*1);
pmmccorkell 0:37123f30e8b2 1113 pc.printf("log: pitch rx: %i\r\n",pit);
pmmccorkell 0:37123f30e8b2 1114 if (pit==999) {
pmmccorkell 0:37123f30e8b2 1115 persistent_pitch=-1;
pmmccorkell 0:37123f30e8b2 1116 pid_pitch.clear();
pmmccorkell 0:37123f30e8b2 1117 }
pmmccorkell 0:37123f30e8b2 1118 if ((pit>=811) and (pit<=817)) {
pmmccorkell 0:37123f30e8b2 1119 increment_persistent(pit10,(pit1-4));
pmmccorkell 0:37123f30e8b2 1120 }
pmmccorkell 0:37123f30e8b2 1121 if ((pit>=0) and (pit<=360)) {
pmmccorkell 0:37123f30e8b2 1122 pid_pitch.clear();
pmmccorkell 0:37123f30e8b2 1123 if (event_horizon_flag==0) persistent_pitch=pit;
pmmccorkell 0:37123f30e8b2 1124 }
pmmccorkell 0:37123f30e8b2 1125 }
pmmccorkell 0:37123f30e8b2 1126 if (verified_tst==3) {
pmmccorkell 0:37123f30e8b2 1127 stop_all_persistent();
pmmccorkell 0:37123f30e8b2 1128 int tst_mode=(input[4]-'0');
pmmccorkell 0:37123f30e8b2 1129 int tst_data_dir=(input[5]-'0');
pmmccorkell 0:37123f30e8b2 1130 int tst_data_move=(input[6]-'0');
pmmccorkell 0:37123f30e8b2 1131 //For both tst_modes, tst_data_msb=0 stops all thrusters.
pmmccorkell 0:37123f30e8b2 1132 //tst_mode 0 is for automated testing of 4 movements (tst_data_msb[0,4]) for fixed 3s.
pmmccorkell 0:37123f30e8b2 1133 if (tst_mode==0) {
pmmccorkell 0:37123f30e8b2 1134 pc.printf("log: tst, mode: %i, dir: %i, move: %i\r\n",tst_mode,tst_data_dir,tst_data_move);
pmmccorkell 0:37123f30e8b2 1135 manual_mode=1;
pmmccorkell 0:37123f30e8b2 1136 test_direction(tst_data_move);
pmmccorkell 0:37123f30e8b2 1137 manual_mode=0;
pmmccorkell 0:37123f30e8b2 1138 }
pmmccorkell 0:37123f30e8b2 1139 //tst_mode 1 is for manual commands of 4 movements for indefinite periods.
pmmccorkell 0:37123f30e8b2 1140 if (tst_mode==1) {
pmmccorkell 0:37123f30e8b2 1141 //Need to create way to exit manual mode
pmmccorkell 0:37123f30e8b2 1142 // presently, have to cheese a tst:0xx command
pmmccorkell 0:37123f30e8b2 1143 manual_mode=1;
pmmccorkell 0:37123f30e8b2 1144 manual_direction(tst_data_dir,tst_data_move);
pmmccorkell 0:37123f30e8b2 1145 }
pmmccorkell 0:37123f30e8b2 1146 }
pmmccorkell 0:37123f30e8b2 1147 if (verified_res==3) {
pmmccorkell 0:37123f30e8b2 1148 pc.printf("log: Reset mbed received. See you on the other side.\r\n");
pmmccorkell 0:37123f30e8b2 1149 //Mbed reset command.
pmmccorkell 0:37123f30e8b2 1150 NVIC_SystemReset();
pmmccorkell 0:37123f30e8b2 1151 //If this print statement is ever executed, reset didn't happen.
pmmccorkell 0:37123f30e8b2 1152 pc.printf("log: Reset failed. The show goes on.\r\n");
pmmccorkell 0:37123f30e8b2 1153 }
pmmccorkell 0:37123f30e8b2 1154 // Forward/Reverse speed commands.
pmmccorkell 0:37123f30e8b2 1155 if (verified_vel==3) {
pmmccorkell 0:37123f30e8b2 1156 int vel100=(input[4]-'0');
pmmccorkell 0:37123f30e8b2 1157 int vel10=(input[5]-'0');
pmmccorkell 0:37123f30e8b2 1158 int vel1=(input[6]-'0');
pmmccorkell 0:37123f30e8b2 1159 int vel=(vel100*100)+(vel10*10)+(vel1*1);
pmmccorkell 0:37123f30e8b2 1160 pc.printf("log: vel rx: %i\r\n",vel);
pmmccorkell 0:37123f30e8b2 1161 //999 to stop speed controller.
pmmccorkell 0:37123f30e8b2 1162 if (vel==999) {
pmmccorkell 0:37123f30e8b2 1163 persistent_speed=-1;
pmmccorkell 0:37123f30e8b2 1164 pid_pitch.clear();
pmmccorkell 0:37123f30e8b2 1165 }
pmmccorkell 0:37123f30e8b2 1166 if ((vel>=841) and (vel<=847)) {
pmmccorkell 0:37123f30e8b2 1167 increment_persistent(vel10,(vel1-4));
pmmccorkell 0:37123f30e8b2 1168 }
pmmccorkell 0:37123f30e8b2 1169 if ((vel>=0) and (vel<=800)) {
pmmccorkell 0:37123f30e8b2 1170 // pid_speed.clear();
pmmccorkell 0:37123f30e8b2 1171 vel=(vel-400);
pmmccorkell 0:37123f30e8b2 1172 if (event_horizon_flag==0) persistent_speed=vel;
pmmccorkell 0:37123f30e8b2 1173 }
pmmccorkell 0:37123f30e8b2 1174 }
pmmccorkell 0:37123f30e8b2 1175 if (verified_zer==3) {
pmmccorkell 0:37123f30e8b2 1176 double zero=press_sensor.set_atm();
pmmccorkell 0:37123f30e8b2 1177 zero_set=1;
pmmccorkell 0:37123f30e8b2 1178 pc.printf("log: zeroized %f Pa\r\n",zero);
pmmccorkell 0:37123f30e8b2 1179 }
pmmccorkell 0:37123f30e8b2 1180 }
pmmccorkell 0:37123f30e8b2 1181 }
pmmccorkell 0:37123f30e8b2 1182
pmmccorkell 0:37123f30e8b2 1183 //Initialize controllers associated with Azimuth, speed and heading.
pmmccorkell 0:37123f30e8b2 1184 void init_AzController(void){
pmmccorkell 0:37123f30e8b2 1185 // Zero out PID values.
pmmccorkell 0:37123f30e8b2 1186 // pid_speed.clear();
pmmccorkell 0:37123f30e8b2 1187 pid_heading.clear();
pmmccorkell 0:37123f30e8b2 1188
pmmccorkell 0:37123f30e8b2 1189 // run Az controller as set in globals, in ms
pmmccorkell 0:37123f30e8b2 1190 tickerAzThrusters.attach(&az_thruster_logic, ticker_rate);
pmmccorkell 0:37123f30e8b2 1191 }
pmmccorkell 0:37123f30e8b2 1192
pmmccorkell 0:37123f30e8b2 1193 //Initialize controllers associated with Elevation, depth and pitch.
pmmccorkell 0:37123f30e8b2 1194 void init_ElController(void){
pmmccorkell 0:37123f30e8b2 1195 // Zero out PID values.
pmmccorkell 0:37123f30e8b2 1196 pid_depth.clear();
pmmccorkell 0:37123f30e8b2 1197 pid_pitch.clear();
pmmccorkell 0:37123f30e8b2 1198
pmmccorkell 0:37123f30e8b2 1199 // run El controller as set in globals, in ms
pmmccorkell 0:37123f30e8b2 1200 tickerElThrusters.attach(&el_thruster_logic, ticker_rate);
pmmccorkell 0:37123f30e8b2 1201 }
pmmccorkell 0:37123f30e8b2 1202
pmmccorkell 0:37123f30e8b2 1203 int main() {
pmmccorkell 0:37123f30e8b2 1204 //engage plaidspeed
pmmccorkell 0:37123f30e8b2 1205 pc.baud(baudrate);
pmmccorkell 0:37123f30e8b2 1206 // press_sensor.init(sensor_rate,0); //02BA pressure sensor
pmmccorkell 0:37123f30e8b2 1207 press_sensor.init(sensor_rate,press_sensor_type); //30BA pressure sensor
pmmccorkell 0:37123f30e8b2 1208 press_sensor.density(997);
pmmccorkell 0:37123f30e8b2 1209
pmmccorkell 0:37123f30e8b2 1210 //Uncomment to derive timing using Oscope.
pmmccorkell 0:37123f30e8b2 1211 //function_timer=0;
pmmccorkell 0:37123f30e8b2 1212 //function_timer2=0;
pmmccorkell 0:37123f30e8b2 1213 //function_timer3=0;
pmmccorkell 0:37123f30e8b2 1214
pmmccorkell 0:37123f30e8b2 1215 //If not ready, reset BNO.
pmmccorkell 0:37123f30e8b2 1216 while (imu.chip_ready() == 0) {
pmmccorkell 0:37123f30e8b2 1217 do {
pmmccorkell 0:37123f30e8b2 1218 pc.printf("resetting BNO\r\n");
pmmccorkell 0:37123f30e8b2 1219 pwr_on=0;
pmmccorkell 0:37123f30e8b2 1220 wait_ms(100);
pmmccorkell 0:37123f30e8b2 1221 pwr_on=1;
pmmccorkell 0:37123f30e8b2 1222 wait_ms(50);
pmmccorkell 0:37123f30e8b2 1223 } while(imu.reset());
pmmccorkell 0:37123f30e8b2 1224 }
pmmccorkell 0:37123f30e8b2 1225 wait_ms(20);
pmmccorkell 0:37123f30e8b2 1226
pmmccorkell 0:37123f30e8b2 1227 //If BNO is ready, set ready status indication
pmmccorkell 0:37123f30e8b2 1228 if (imu.chip_ready()) {
pmmccorkell 0:37123f30e8b2 1229 ready_prefix=0xffff;
pmmccorkell 0:37123f30e8b2 1230 }
pmmccorkell 0:37123f30e8b2 1231 switch_pos(1); //BNO default position 1
pmmccorkell 0:37123f30e8b2 1232
pmmccorkell 0:37123f30e8b2 1233 init_AzController();
pmmccorkell 0:37123f30e8b2 1234 init_ElController();
pmmccorkell 0:37123f30e8b2 1235
pmmccorkell 0:37123f30e8b2 1236 //Look for serial input commands and send to 'commands' function.
pmmccorkell 0:37123f30e8b2 1237 //If no serial input commands, stream data.
pmmccorkell 0:37123f30e8b2 1238 while(1) {
pmmccorkell 0:37123f30e8b2 1239 if (pc.readable()) {
pmmccorkell 0:37123f30e8b2 1240 // command_available=0;
pmmccorkell 0:37123f30e8b2 1241 commands();
pmmccorkell 0:37123f30e8b2 1242 //function_timer4=1;
pmmccorkell 0:37123f30e8b2 1243 // command_available=1;
pmmccorkell 0:37123f30e8b2 1244 }
pmmccorkell 0:37123f30e8b2 1245 else {
pmmccorkell 0:37123f30e8b2 1246 az_data();
pmmccorkell 0:37123f30e8b2 1247 }
pmmccorkell 0:37123f30e8b2 1248 wait_ms(wait_main/2);
pmmccorkell 0:37123f30e8b2 1249 //function_timer4=0;
pmmccorkell 0:37123f30e8b2 1250 wait_ms(wait_main/2);
pmmccorkell 0:37123f30e8b2 1251 }
pmmccorkell 0:37123f30e8b2 1252 }
pmmccorkell 0:37123f30e8b2 1253