Pat McC
/
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@4:e89234ca9d4e, 2020-11-12 (annotated)
- Committer:
- pmmccorkell
- Date:
- Thu Nov 12 20:25:07 2020 +0000
- Revision:
- 4:e89234ca9d4e
- Parent:
- 3:d6471216e378
asf
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| pmmccorkell | 0:37123f30e8b2 | 1 | #include "mbed.h" |
| pmmccorkell | 0:37123f30e8b2 | 2 | #include "BNO055.h" //imu |
| pmmccorkell | 0:37123f30e8b2 | 3 | #include "PID.h" |
| pmmccorkell | 4:e89234ca9d4e | 4 | #define PI_math 3.14159265358979323846 |
| pmmccorkell | 4:e89234ca9d4e | 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 | int wait_main=20; //ms to wait in main, and associated, loops |
| pmmccorkell | 0:37123f30e8b2 | 16 | |
| pmmccorkell | 0:37123f30e8b2 | 17 | /* |
| pmmccorkell | 0:37123f30e8b2 | 18 | // IO pins used with oscope to observe and gather timing data of the program itself |
| pmmccorkell | 0:37123f30e8b2 | 19 | DigitalOut function_timer(p5); //el logic + depth and pitch controllers |
| pmmccorkell | 0:37123f30e8b2 | 20 | DigitalOut function_timer2(p6); //sensor update and data stream |
| pmmccorkell | 0:37123f30e8b2 | 21 | DigitalOut function_timer3(p7); //az logic + heading and speed controllers |
| pmmccorkell | 0:37123f30e8b2 | 22 | DigitalOut function_timer4(p8); //high edge when a command is read and completed execution. |
| pmmccorkell | 0:37123f30e8b2 | 23 | //Pair with RasPi GPIO and use Oscope to time how long it |
| pmmccorkell | 0:37123f30e8b2 | 24 | //takes to send, receive, and execute commands. |
| pmmccorkell | 0:37123f30e8b2 | 25 | */ |
| pmmccorkell | 0:37123f30e8b2 | 26 | |
| pmmccorkell | 0:37123f30e8b2 | 27 | DigitalIn leak_detect(p11); |
| pmmccorkell | 0:37123f30e8b2 | 28 | DigitalOut leak_light(LED1); |
| pmmccorkell | 0:37123f30e8b2 | 29 | |
| pmmccorkell | 0:37123f30e8b2 | 30 | // ESC specs data |
| pmmccorkell | 0:37123f30e8b2 | 31 | double esc_freq=400; //Standard servo rate -> 400Hz |
| pmmccorkell | 0:37123f30e8b2 | 32 | double base_pw=1.5; //ms |
| pmmccorkell | 0:37123f30e8b2 | 33 | double null_pw=0.0; |
| pmmccorkell | 0:37123f30e8b2 | 34 | double esc_period=(1000/esc_freq); //ms |
| pmmccorkell | 0:37123f30e8b2 | 35 | double esc_range_spec=.4; // 400ms, [1.1,1.9]ms pw per data sheet |
| pmmccorkell | 0:37123f30e8b2 | 36 | double esc_range_scale=1.0; //only use x% of full range |
| pmmccorkell | 0:37123f30e8b2 | 37 | double esc_range=esc_range_spec*esc_range_scale; |
| pmmccorkell | 0:37123f30e8b2 | 38 | double min_thruster_bias=0.028; //deadzone around 1.5ms where ESC will not operate |
| pmmccorkell | 0:37123f30e8b2 | 39 | double pw_tolerance=0.00001; |
| pmmccorkell | 0:37123f30e8b2 | 40 | |
| pmmccorkell | 0:37123f30e8b2 | 41 | volatile float ticker_rate= 0.02; //Time between ticker calls in seconds. |
| pmmccorkell | 0:37123f30e8b2 | 42 | float target_time=5; //Ideal timeframe to reach desired heading, depth, pitch using only Kp. |
| pmmccorkell | 0:37123f30e8b2 | 43 | float target_time_multiple=2; //Ki will max out PWM in "factor * target_time". |
| pmmccorkell | 0:37123f30e8b2 | 44 | |
| pmmccorkell | 0:37123f30e8b2 | 45 | // |
| pmmccorkell | 0:37123f30e8b2 | 46 | //Azimuth controllers |
| pmmccorkell | 0:37123f30e8b2 | 47 | // |
| pmmccorkell | 0:37123f30e8b2 | 48 | Ticker tickerAzThrusters; |
| pmmccorkell | 0:37123f30e8b2 | 49 | //degrees of acceptable heading tolerance |
| pmmccorkell | 0:37123f30e8b2 | 50 | double az_tolerance=2; |
| pmmccorkell | 0:37123f30e8b2 | 51 | // PID gains for heading |
| pmmccorkell | 4:e89234ca9d4e | 52 | volatile float heading_Kp = 0.0003333f; //0.0008333f; |
| pmmccorkell | 0:37123f30e8b2 | 53 | volatile float heading_Ki =0.0f; |
| pmmccorkell | 0:37123f30e8b2 | 54 | volatile float heading_Kd =0.0f; |
| pmmccorkell | 0:37123f30e8b2 | 55 | PID pid_heading(heading_Kp,heading_Ki,heading_Kd,ticker_rate,az_tolerance); |
| pmmccorkell | 0:37123f30e8b2 | 56 | // PID gains for speed |
| pmmccorkell | 0:37123f30e8b2 | 57 | // volatile float speed_Kp=1.0f; |
| pmmccorkell | 0:37123f30e8b2 | 58 | // volatile float speed_Ki=0.0f; |
| pmmccorkell | 0:37123f30e8b2 | 59 | // volatile float speed_Kd=0.0; |
| pmmccorkell | 0:37123f30e8b2 | 60 | // PID pid_speed(speed_Kp, speed_Ki, speed_Kd, ticker_rate, speed_tolerance); |
| pmmccorkell | 0:37123f30e8b2 | 61 | |
| pmmccorkell | 0:37123f30e8b2 | 62 | //instantiate globals for sensor updates and data stream |
| pmmccorkell | 0:37123f30e8b2 | 63 | uint16_t ready_prefix = 0x0000; |
| pmmccorkell | 0:37123f30e8b2 | 64 | uint16_t horizon_prefix=0xff00; |
| pmmccorkell | 0:37123f30e8b2 | 65 | uint16_t ready_data = 0x0000; |
| pmmccorkell | 0:37123f30e8b2 | 66 | uint16_t heading = 0xffff; |
| pmmccorkell | 0:37123f30e8b2 | 67 | uint16_t pitch = 0xffff; |
| pmmccorkell | 0:37123f30e8b2 | 68 | char readline[100]; |
| pmmccorkell | 0:37123f30e8b2 | 69 | |
| pmmccorkell | 0:37123f30e8b2 | 70 | //instantiate globals for flags and function indication |
| pmmccorkell | 0:37123f30e8b2 | 71 | int command_available=1; |
| pmmccorkell | 0:37123f30e8b2 | 72 | int call_threads_available=1; |
| pmmccorkell | 0:37123f30e8b2 | 73 | int logic_available=1; |
| pmmccorkell | 0:37123f30e8b2 | 74 | int manual_mode=0; |
| pmmccorkell | 0:37123f30e8b2 | 75 | int zero_set=0; |
| pmmccorkell | 0:37123f30e8b2 | 76 | int horizon_count=0; |
| pmmccorkell | 0:37123f30e8b2 | 77 | int event_horizon_flag=0; |
| pmmccorkell | 0:37123f30e8b2 | 78 | |
| pmmccorkell | 0:37123f30e8b2 | 79 | //instantiate goto position globals |
| pmmccorkell | 0:37123f30e8b2 | 80 | //-1 indicates no setting |
| pmmccorkell | 0:37123f30e8b2 | 81 | int persistent_heading=-1; |
| pmmccorkell | 0:37123f30e8b2 | 82 | int persistent_speed=-1; |
| pmmccorkell | 4:e89234ca9d4e | 83 | int persistent_offset=-1; |
| pmmccorkell | 0:37123f30e8b2 | 84 | |
| pmmccorkell | 0:37123f30e8b2 | 85 | BNO055_ID_INF_TypeDef bno055_id_inf; |
| pmmccorkell | 0:37123f30e8b2 | 86 | BNO055_EULER_TypeDef euler_angles; |
| pmmccorkell | 4:e89234ca9d4e | 87 | BNO055_QUATERNION_TypeDef quaternion; |
| pmmccorkell | 0:37123f30e8b2 | 88 | //BNO055_LIN_ACC_TypeDef linear_acc; |
| pmmccorkell | 0:37123f30e8b2 | 89 | //BNO055_GRAVITY_TypeDef gravity; |
| pmmccorkell | 0:37123f30e8b2 | 90 | //BNO055_TEMPERATURE_TypeDef chip_temp; |
| pmmccorkell | 0:37123f30e8b2 | 91 | |
| pmmccorkell | 4:e89234ca9d4e | 92 | typedef struct { |
| pmmccorkell | 4:e89234ca9d4e | 93 | uint16_t h; |
| pmmccorkell | 4:e89234ca9d4e | 94 | uint16_t r; |
| pmmccorkell | 4:e89234ca9d4e | 95 | uint16_t p; |
| pmmccorkell | 4:e89234ca9d4e | 96 | } CONVERTED_QUATERNION_TypeDef; |
| pmmccorkell | 4:e89234ca9d4e | 97 | |
| pmmccorkell | 4:e89234ca9d4e | 98 | CONVERTED_QUATERNION_TypeDef conversion; |
| pmmccorkell | 0:37123f30e8b2 | 99 | |
| pmmccorkell | 0:37123f30e8b2 | 100 | //-----THRUSTER CLASS BEGIN-----// |
| pmmccorkell | 0:37123f30e8b2 | 101 | //Thruster class to instantiate individual thrusters. |
| pmmccorkell | 0:37123f30e8b2 | 102 | class Thruster { |
| pmmccorkell | 0:37123f30e8b2 | 103 | public: |
| pmmccorkell | 0:37123f30e8b2 | 104 | Thruster(PinName pin, float dir); |
| pmmccorkell | 0:37123f30e8b2 | 105 | void setEvent(); |
| pmmccorkell | 0:37123f30e8b2 | 106 | void clearEvent(); |
| pmmccorkell | 0:37123f30e8b2 | 107 | int available(); |
| pmmccorkell | 0:37123f30e8b2 | 108 | void set_period(double thruster_time); |
| pmmccorkell | 0:37123f30e8b2 | 109 | void set_pw(double thruster_pw); |
| pmmccorkell | 0:37123f30e8b2 | 110 | double get_pw(); |
| pmmccorkell | 0:37123f30e8b2 | 111 | double get_speed(); |
| pmmccorkell | 0:37123f30e8b2 | 112 | uint32_t thruster_data(); |
| pmmccorkell | 0:37123f30e8b2 | 113 | void set_speed(double pntr); |
| pmmccorkell | 0:37123f30e8b2 | 114 | protected: |
| pmmccorkell | 0:37123f30e8b2 | 115 | PwmOut _pwm; |
| pmmccorkell | 0:37123f30e8b2 | 116 | PinName _pin; |
| pmmccorkell | 0:37123f30e8b2 | 117 | float _d; |
| pmmccorkell | 0:37123f30e8b2 | 118 | int _lock; |
| pmmccorkell | 0:37123f30e8b2 | 119 | int _available; |
| pmmccorkell | 0:37123f30e8b2 | 120 | double _base_pw, _period; |
| pmmccorkell | 0:37123f30e8b2 | 121 | }; |
| pmmccorkell | 0:37123f30e8b2 | 122 | |
| pmmccorkell | 0:37123f30e8b2 | 123 | //Instantiation accepts PWM pin and direction |
| pmmccorkell | 0:37123f30e8b2 | 124 | //Direction is -1 or 1. 1 for normal, -1 if blade reversed. |
| pmmccorkell | 0:37123f30e8b2 | 125 | Thruster::Thruster(PinName pin, float dir) : _pwm(pin), _d(dir) { |
| pmmccorkell | 0:37123f30e8b2 | 126 | _lock=0; |
| pmmccorkell | 0:37123f30e8b2 | 127 | _available=1; |
| pmmccorkell | 0:37123f30e8b2 | 128 | _pin=pin; |
| pmmccorkell | 0:37123f30e8b2 | 129 | _base_pw=1.5; |
| pmmccorkell | 0:37123f30e8b2 | 130 | set_pw(_base_pw); |
| pmmccorkell | 0:37123f30e8b2 | 131 | _period=2.5; |
| pmmccorkell | 0:37123f30e8b2 | 132 | set_period(_period); |
| pmmccorkell | 0:37123f30e8b2 | 133 | //pc.printf("Thruster: %f\r\n",d); |
| pmmccorkell | 0:37123f30e8b2 | 134 | } |
| pmmccorkell | 0:37123f30e8b2 | 135 | |
| pmmccorkell | 0:37123f30e8b2 | 136 | //Sets Event for Emergency Stop and sets lockout to set_speed() function. |
| pmmccorkell | 0:37123f30e8b2 | 137 | void Thruster::setEvent() { |
| pmmccorkell | 0:37123f30e8b2 | 138 | _lock=1; |
| pmmccorkell | 0:37123f30e8b2 | 139 | set_pw(_base_pw); |
| pmmccorkell | 0:37123f30e8b2 | 140 | } |
| pmmccorkell | 0:37123f30e8b2 | 141 | |
| pmmccorkell | 0:37123f30e8b2 | 142 | //Clears Event for Emergency Stop of thruster and removes lockout from set_speed() function. |
| pmmccorkell | 0:37123f30e8b2 | 143 | void Thruster::clearEvent() { |
| pmmccorkell | 0:37123f30e8b2 | 144 | _lock=0; |
| pmmccorkell | 0:37123f30e8b2 | 145 | } |
| pmmccorkell | 0:37123f30e8b2 | 146 | |
| pmmccorkell | 0:37123f30e8b2 | 147 | //Returns whether set_speed() function is available, or currently in use. |
| pmmccorkell | 0:37123f30e8b2 | 148 | int Thruster::available() { |
| pmmccorkell | 0:37123f30e8b2 | 149 | return _available; |
| pmmccorkell | 0:37123f30e8b2 | 150 | } |
| pmmccorkell | 0:37123f30e8b2 | 151 | |
| pmmccorkell | 0:37123f30e8b2 | 152 | //Set PWM period in ms. |
| pmmccorkell | 0:37123f30e8b2 | 153 | void Thruster::set_period(double thruster_time) { |
| pmmccorkell | 0:37123f30e8b2 | 154 | _period=thruster_time; |
| pmmccorkell | 0:37123f30e8b2 | 155 | _pwm.period(_period/1000); |
| pmmccorkell | 0:37123f30e8b2 | 156 | } |
| pmmccorkell | 0:37123f30e8b2 | 157 | |
| pmmccorkell | 0:37123f30e8b2 | 158 | //Set PWM pulsewidth in ms |
| pmmccorkell | 0:37123f30e8b2 | 159 | void Thruster::set_pw(double thruster_pw) { |
| pmmccorkell | 0:37123f30e8b2 | 160 | double s_pw=(thruster_pw/1000); |
| pmmccorkell | 0:37123f30e8b2 | 161 | pc.printf("log: set_pw: %f\r\n",s_pw); |
| pmmccorkell | 0:37123f30e8b2 | 162 | _pwm.pulsewidth(s_pw); |
| pmmccorkell | 0:37123f30e8b2 | 163 | } |
| pmmccorkell | 0:37123f30e8b2 | 164 | |
| pmmccorkell | 0:37123f30e8b2 | 165 | //Returns PWM pulsewidth in ms. |
| pmmccorkell | 0:37123f30e8b2 | 166 | double Thruster::get_pw() { |
| pmmccorkell | 0:37123f30e8b2 | 167 | //read duty cycle times period |
| pmmccorkell | 0:37123f30e8b2 | 168 | double g_pw = (_pwm.read()*_period); |
| pmmccorkell | 0:37123f30e8b2 | 169 | //pc.printf(" get_pw: %f, ",g_pw); |
| pmmccorkell | 0:37123f30e8b2 | 170 | return g_pw; |
| pmmccorkell | 0:37123f30e8b2 | 171 | } |
| pmmccorkell | 0:37123f30e8b2 | 172 | |
| pmmccorkell | 0:37123f30e8b2 | 173 | //Returns PWM output relative to 1.5ms. |
| pmmccorkell | 0:37123f30e8b2 | 174 | double Thruster::get_speed() { |
| pmmccorkell | 0:37123f30e8b2 | 175 | double g_speed = (get_pw()-_base_pw); |
| pmmccorkell | 0:37123f30e8b2 | 176 | //pc.printf("get_speed: %f, ",g_speed); |
| pmmccorkell | 0:37123f30e8b2 | 177 | return g_speed; |
| pmmccorkell | 0:37123f30e8b2 | 178 | } |
| pmmccorkell | 0:37123f30e8b2 | 179 | |
| pmmccorkell | 0:37123f30e8b2 | 180 | //formats PWM as an 2 uint16_t joined to make uint32_t for serial data streaming |
| pmmccorkell | 0:37123f30e8b2 | 181 | //MSB uint16_t indicates direction, 0 for positive, 1 for negative. |
| pmmccorkell | 0:37123f30e8b2 | 182 | //LSB uint16_t is 10ms resolution of PWM |
| pmmccorkell | 0:37123f30e8b2 | 183 | uint32_t Thruster::thruster_data() { |
| pmmccorkell | 0:37123f30e8b2 | 184 | double speed=get_speed(); |
| pmmccorkell | 0:37123f30e8b2 | 185 | uint32_t dir=0x0; |
| pmmccorkell | 0:37123f30e8b2 | 186 | uint32_t data=0x0; |
| pmmccorkell | 0:37123f30e8b2 | 187 | if (speed<0) dir =0x00010000; |
| pmmccorkell | 0:37123f30e8b2 | 188 | data=static_cast<unsigned int>(abs(int(speed*100000))); |
| pmmccorkell | 0:37123f30e8b2 | 189 | data=data+dir; |
| pmmccorkell | 0:37123f30e8b2 | 190 | return data; |
| pmmccorkell | 0:37123f30e8b2 | 191 | } |
| pmmccorkell | 0:37123f30e8b2 | 192 | |
| pmmccorkell | 0:37123f30e8b2 | 193 | //Accepts adjustment to pw [-500,500] ms that is added to 1.5ms |
| pmmccorkell | 0:37123f30e8b2 | 194 | void Thruster::set_speed(double speed_pw) { |
| pmmccorkell | 0:37123f30e8b2 | 195 | if (_lock==1) { |
| pmmccorkell | 0:37123f30e8b2 | 196 | set_pw(_base_pw); |
| pmmccorkell | 0:37123f30e8b2 | 197 | } |
| pmmccorkell | 0:37123f30e8b2 | 198 | else { |
| pmmccorkell | 0:37123f30e8b2 | 199 | double tolerance_pw=0.001; |
| pmmccorkell | 0:37123f30e8b2 | 200 | double target_pw=(_d*speed_pw)+_base_pw; |
| pmmccorkell | 0:37123f30e8b2 | 201 | double current_pw=get_pw(); |
| pmmccorkell | 0:37123f30e8b2 | 202 | double diff_pw=abs(target_pw-current_pw); |
| pmmccorkell | 0:37123f30e8b2 | 203 | if (diff_pw>tolerance_pw) set_pw(target_pw); |
| pmmccorkell | 0:37123f30e8b2 | 204 | } |
| pmmccorkell | 0:37123f30e8b2 | 205 | } |
| pmmccorkell | 0:37123f30e8b2 | 206 | //-----THRUSTER CLASS END-----// |
| pmmccorkell | 0:37123f30e8b2 | 207 | |
| pmmccorkell | 0:37123f30e8b2 | 208 | // Instantiate thrusters. |
| pmmccorkell | 4:e89234ca9d4e | 209 | Thruster fwd_port(p21,1); |
| pmmccorkell | 4:e89234ca9d4e | 210 | Thruster fwd_star(p22,-1); |
| pmmccorkell | 0:37123f30e8b2 | 211 | //Thruster steadystate(p23,1); //for test purposes, to keep ESC from making LOUD NOISES |
| pmmccorkell | 4:e89234ca9d4e | 212 | Thruster aft_port(p24,-1); |
| pmmccorkell | 4:e89234ca9d4e | 213 | Thruster aft_star(p25,1); |
| pmmccorkell | 0:37123f30e8b2 | 214 | |
| pmmccorkell | 0:37123f30e8b2 | 215 | //Function to check for water leak. Open is good, short is bad. |
| pmmccorkell | 0:37123f30e8b2 | 216 | void leak_data() { |
| pmmccorkell | 0:37123f30e8b2 | 217 | leak_detect.read(); |
| pmmccorkell | 0:37123f30e8b2 | 218 | if (leak_detect==1) { |
| pmmccorkell | 0:37123f30e8b2 | 219 | ready_data=(ready_data | 0x0800); |
| pmmccorkell | 0:37123f30e8b2 | 220 | } |
| pmmccorkell | 0:37123f30e8b2 | 221 | leak_light=leak_detect; |
| pmmccorkell | 0:37123f30e8b2 | 222 | } |
| pmmccorkell | 0:37123f30e8b2 | 223 | |
| pmmccorkell | 0:37123f30e8b2 | 224 | //Function to format Gain values into 3 significant digits and exponent value. |
| pmmccorkell | 0:37123f30e8b2 | 225 | // ie 0x abc * 10^(d-9) -> 0x abcd |
| pmmccorkell | 0:37123f30e8b2 | 226 | uint16_t k_data(float k_val) { |
| pmmccorkell | 0:37123f30e8b2 | 227 | |
| pmmccorkell | 0:37123f30e8b2 | 228 | // Static lookup table for 10^11 to 10^-4. |
| pmmccorkell | 0:37123f30e8b2 | 229 | // Arrived at for range to get 3 significant digits for values from 10^-9 to 10^6, respectively. |
| pmmccorkell | 0:37123f30e8b2 | 230 | static double pow10[16] = { |
| pmmccorkell | 0:37123f30e8b2 | 231 | 100000000000, 10000000000, 1000000000, 100000000, //[0,4] |
| pmmccorkell | 0:37123f30e8b2 | 232 | 10000000, 1000000, 100000, 10000, //[5,8] |
| pmmccorkell | 0:37123f30e8b2 | 233 | 1000, 100, 10, 1, //[9,12] |
| pmmccorkell | 0:37123f30e8b2 | 234 | 0.1, 0.01, 0.001, 0.0001, //[13,16] |
| pmmccorkell | 0:37123f30e8b2 | 235 | }; |
| pmmccorkell | 0:37123f30e8b2 | 236 | |
| pmmccorkell | 0:37123f30e8b2 | 237 | // Find where significant digits start and get the exponent value. |
| pmmccorkell | 0:37123f30e8b2 | 238 | int exponent_value = floor(log10(k_val)); |
| pmmccorkell | 0:37123f30e8b2 | 239 | |
| pmmccorkell | 0:37123f30e8b2 | 240 | // Form scalar value of 3 significant digits. |
| pmmccorkell | 0:37123f30e8b2 | 241 | int sig_val = floor((k_val*pow10[exponent_value+9])+0.5); |
| pmmccorkell | 0:37123f30e8b2 | 242 | |
| pmmccorkell | 0:37123f30e8b2 | 243 | // Shift exponent so that 10^-9 starts at 0. |
| pmmccorkell | 0:37123f30e8b2 | 244 | exponent_value+=9; |
| pmmccorkell | 0:37123f30e8b2 | 245 | |
| pmmccorkell | 0:37123f30e8b2 | 246 | // Move Scalar left 1 full hex, and append exponent to the end. |
| pmmccorkell | 0:37123f30e8b2 | 247 | uint16_t return_val=(sig_val<<4)+(exponent_value); |
| pmmccorkell | 0:37123f30e8b2 | 248 | |
| pmmccorkell | 0:37123f30e8b2 | 249 | //Return the formed hex value. |
| pmmccorkell | 0:37123f30e8b2 | 250 | return return_val; |
| pmmccorkell | 0:37123f30e8b2 | 251 | } |
| pmmccorkell | 0:37123f30e8b2 | 252 | |
| pmmccorkell | 4:e89234ca9d4e | 253 | |
| pmmccorkell | 4:e89234ca9d4e | 254 | //Convert Quaternion to Euler angles |
| pmmccorkell | 4:e89234ca9d4e | 255 | //https://forums.adafruit.com/viewtopic.php?f=19&t=95266 |
| pmmccorkell | 4:e89234ca9d4e | 256 | void q_to_E(double qw, double qx, double qy, double qz, CONVERTED_QUATERNION_TypeDef *conv) { |
| pmmccorkell | 4:e89234ca9d4e | 257 | double qnorm = 1 / sqrt((double)(qw*qw + qx*qx + qy*qy + qz*qz)); // normalize the quaternion |
| pmmccorkell | 4:e89234ca9d4e | 258 | qw *= qnorm; |
| pmmccorkell | 4:e89234ca9d4e | 259 | qx *= qnorm; |
| pmmccorkell | 4:e89234ca9d4e | 260 | qy *= qnorm; |
| pmmccorkell | 4:e89234ca9d4e | 261 | qz *= qnorm; |
| pmmccorkell | 4:e89234ca9d4e | 262 | double factor = 180.0/PI_math; |
| pmmccorkell | 4:e89234ca9d4e | 263 | double roll_conversion = factor * atan2((double)(qw*qx + qy*qz), (double)(0.5 - qx*qx - qy*qy)); |
| pmmccorkell | 4:e89234ca9d4e | 264 | //double pitch = factor * asin((double)(2 * (qw*qy - qx*qz))); //reversed |
| pmmccorkell | 4:e89234ca9d4e | 265 | double pitch_conversion = factor * asin((double)(2 * (qx*qz - qw*qy))); |
| pmmccorkell | 4:e89234ca9d4e | 266 | double heading_conversion = factor * atan2((double)(qw*qz + qx*qy), (double)(0.5 - qy*qy - qz*qz)); |
| pmmccorkell | 4:e89234ca9d4e | 267 | heading_conversion=(-1*heading_conversion); |
| pmmccorkell | 4:e89234ca9d4e | 268 | if (heading_conversion<0) { |
| pmmccorkell | 4:e89234ca9d4e | 269 | heading_conversion+=360; //quaternion reads heading in CCW, flip to CW |
| pmmccorkell | 4:e89234ca9d4e | 270 | } |
| pmmccorkell | 4:e89234ca9d4e | 271 | //pc.printf("log: Q-derived h: %lf, r: %lf, P: %lf\n\r", heading_conversion, roll_conversion, pitch_conversion); |
| pmmccorkell | 4:e89234ca9d4e | 272 | conv->r = int(roll_conversion*16); |
| pmmccorkell | 4:e89234ca9d4e | 273 | conv->h = int(heading_conversion*16); |
| pmmccorkell | 4:e89234ca9d4e | 274 | conv->p = int(pitch_conversion*16); |
| pmmccorkell | 4:e89234ca9d4e | 275 | } |
| pmmccorkell | 4:e89234ca9d4e | 276 | |
| pmmccorkell | 0:37123f30e8b2 | 277 | //Data function pulls data from BNO and sends over serial |
| pmmccorkell | 0:37123f30e8b2 | 278 | //Timed using DigitalOut and Oscope. |
| pmmccorkell | 0:37123f30e8b2 | 279 | // At baud 115200, averaged 5 times over 256, +pulsewidth 11.1 - 13.3ms. |
| pmmccorkell | 0:37123f30e8b2 | 280 | // At baud 921600, averaged over 256, +pw 4.1 - 5.5ms |
| pmmccorkell | 0:37123f30e8b2 | 281 | // In water, 921600 induced serial comm errors |
| pmmccorkell | 0:37123f30e8b2 | 282 | // Variance is due to MS5837 pressure sensor. Includes waits of 2-4ms. |
| pmmccorkell | 0:37123f30e8b2 | 283 | void az_data() { |
| pmmccorkell | 0:37123f30e8b2 | 284 | //function_timer2=1; |
| pmmccorkell | 0:37123f30e8b2 | 285 | |
| pmmccorkell | 0:37123f30e8b2 | 286 | leak_data(); |
| pmmccorkell | 0:37123f30e8b2 | 287 | uint32_t k=0x1234abcd; |
| pmmccorkell | 0:37123f30e8b2 | 288 | |
| pmmccorkell | 0:37123f30e8b2 | 289 | // if (logic_available==1) ready_data=(ready_data&0xfbff); |
| pmmccorkell | 0:37123f30e8b2 | 290 | // else ready_data=(ready_data|0x0400); |
| pmmccorkell | 0:37123f30e8b2 | 291 | if (call_threads_available==1) ready_data=(ready_data & 0xfdff); |
| pmmccorkell | 0:37123f30e8b2 | 292 | else ready_data=(ready_data | 0x0200); |
| pmmccorkell | 0:37123f30e8b2 | 293 | // if (command_available==1) ready_data=(ready_data&0xfeff); |
| pmmccorkell | 0:37123f30e8b2 | 294 | // else ready_data=(ready_data | 0x0100); |
| pmmccorkell | 0:37123f30e8b2 | 295 | if (zero_set==1) ready_data=(ready_data|0x0008); |
| pmmccorkell | 0:37123f30e8b2 | 296 | else ready_data=(ready_data&0xfff7); |
| pmmccorkell | 0:37123f30e8b2 | 297 | |
| pmmccorkell | 0:37123f30e8b2 | 298 | //Instantiate status array of 7 32-bit words. |
| pmmccorkell | 0:37123f30e8b2 | 299 | //First 16 bits of each 32-bit word are Identifiers for RasPi to correctly assign the trailing 16 bits of data. |
| pmmccorkell | 0:37123f30e8b2 | 300 | uint32_t status[14]={0}; |
| pmmccorkell | 0:37123f30e8b2 | 301 | uint32_t ready=ready_prefix; |
| pmmccorkell | 0:37123f30e8b2 | 302 | ready=(ready<<16)|ready_data; |
| pmmccorkell | 0:37123f30e8b2 | 303 | |
| pmmccorkell | 0:37123f30e8b2 | 304 | //word 0: Key |
| pmmccorkell | 0:37123f30e8b2 | 305 | //Used to ensure Pi and Mbed are on same page. |
| pmmccorkell | 0:37123f30e8b2 | 306 | status[0]=k; |
| pmmccorkell | 0:37123f30e8b2 | 307 | |
| pmmccorkell | 0:37123f30e8b2 | 308 | //word 1: Status information. |
| pmmccorkell | 0:37123f30e8b2 | 309 | //0xffff acts as prefix to identify Status for RasPi. |
| pmmccorkell | 0:37123f30e8b2 | 310 | //Last 3 bits (from right) are current position (POS[0-7]). See BNO datasheet. |
| pmmccorkell | 0:37123f30e8b2 | 311 | //4th bit (from right) is RH turn motors enabled. |
| pmmccorkell | 0:37123f30e8b2 | 312 | //5th bit (from right) is LH turn motors enabled. |
| pmmccorkell | 0:37123f30e8b2 | 313 | status[1]=ready; |
| pmmccorkell | 0:37123f30e8b2 | 314 | |
| pmmccorkell | 0:37123f30e8b2 | 315 | //word 2: Calibration. |
| pmmccorkell | 0:37123f30e8b2 | 316 | //0xc000 acts as prefix to identify Cal for RasPi. |
| pmmccorkell | 0:37123f30e8b2 | 317 | status[2]=0xc0000000+imu.read_calib_status(); |
| pmmccorkell | 0:37123f30e8b2 | 318 | |
| pmmccorkell | 0:37123f30e8b2 | 319 | //Get Euler data from BNO. |
| pmmccorkell | 4:e89234ca9d4e | 320 | // imu.get_Euler_Angles(&euler_angles); |
| pmmccorkell | 4:e89234ca9d4e | 321 | imu.get_quaternion(&quaternion); |
| pmmccorkell | 4:e89234ca9d4e | 322 | double qw=(double)quaternion.w; |
| pmmccorkell | 4:e89234ca9d4e | 323 | double qx=(double)quaternion.x; |
| pmmccorkell | 4:e89234ca9d4e | 324 | double qy=(double)quaternion.y; |
| pmmccorkell | 4:e89234ca9d4e | 325 | double qz=(double)quaternion.z; |
| pmmccorkell | 4:e89234ca9d4e | 326 | q_to_E(qw,qx,qy,qz,&conversion); |
| pmmccorkell | 0:37123f30e8b2 | 327 | |
| pmmccorkell | 0:37123f30e8b2 | 328 | //word 3 is Heading. |
| pmmccorkell | 0:37123f30e8b2 | 329 | //0xc100 acts as prefix to identify Heading for RasPi. |
| pmmccorkell | 4:e89234ca9d4e | 330 | //uint16_t h = euler_angles.h; |
| pmmccorkell | 4:e89234ca9d4e | 331 | uint16_t h = conversion.h; |
| pmmccorkell | 0:37123f30e8b2 | 332 | heading=h; |
| pmmccorkell | 0:37123f30e8b2 | 333 | status[3]=0xc1000000+h; |
| pmmccorkell | 0:37123f30e8b2 | 334 | |
| pmmccorkell | 0:37123f30e8b2 | 335 | //Offset calculation: 360*16bit resolution = 5760 -> converted to hex = 0x1680 |
| pmmccorkell | 0:37123f30e8b2 | 336 | int offset=0x1680; |
| pmmccorkell | 0:37123f30e8b2 | 337 | |
| pmmccorkell | 0:37123f30e8b2 | 338 | //word 4 is Roll. |
| pmmccorkell | 0:37123f30e8b2 | 339 | //0xc300 acts as prefix to identify Roll for RasPi. |
| pmmccorkell | 0:37123f30e8b2 | 340 | //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial. |
| pmmccorkell | 4:e89234ca9d4e | 341 | //uint16_t r = offset + euler_angles.r; |
| pmmccorkell | 4:e89234ca9d4e | 342 | uint16_t r = offset + conversion.r; |
| pmmccorkell | 0:37123f30e8b2 | 343 | status[4]=0xc3000000+r; |
| pmmccorkell | 0:37123f30e8b2 | 344 | |
| pmmccorkell | 0:37123f30e8b2 | 345 | //word 5 is Pitch. |
| pmmccorkell | 0:37123f30e8b2 | 346 | //0xc500 acts as prefix to identify Pitch for RasPi. |
| pmmccorkell | 0:37123f30e8b2 | 347 | //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial. |
| pmmccorkell | 4:e89234ca9d4e | 348 | //uint16_t p = offset + euler_angles.p; |
| pmmccorkell | 4:e89234ca9d4e | 349 | uint16_t p = offset + conversion.p; |
| pmmccorkell | 0:37123f30e8b2 | 350 | pitch=(p - (offset/2)); //only want 180deg offset |
| pmmccorkell | 0:37123f30e8b2 | 351 | status[5]=0xc5000000+p; |
| pmmccorkell | 0:37123f30e8b2 | 352 | |
| pmmccorkell | 0:37123f30e8b2 | 353 | //word 6 gets Depth from el_data() function. |
| pmmccorkell | 0:37123f30e8b2 | 354 | //0xb0 acts as prefix to identify Barometer Pressure. |
| pmmccorkell | 4:e89234ca9d4e | 355 | // status[6]=el_data(); |
| pmmccorkell | 0:37123f30e8b2 | 356 | |
| pmmccorkell | 0:37123f30e8b2 | 357 | //Thruster speeds in 10us resolution. |
| pmmccorkell | 4:e89234ca9d4e | 358 | status[6]=((fwd_port.thruster_data() & 0x00ffffff) | 0xf1000000); |
| pmmccorkell | 4:e89234ca9d4e | 359 | status[7]=((fwd_star.thruster_data() &0x00ffffff) | 0xf2000000); |
| pmmccorkell | 4:e89234ca9d4e | 360 | status[8]=((aft_port.thruster_data() & 0x00ffffff) | 0xf3000000); |
| pmmccorkell | 4:e89234ca9d4e | 361 | status[9]=((aft_star.thruster_data() & 0x00ffffff) | 0xf4000000); |
| pmmccorkell | 0:37123f30e8b2 | 362 | |
| pmmccorkell | 0:37123f30e8b2 | 363 | //Gains in format: ABC * 10^(D-9), similar to resistor color bands but starting at 10^-9 rather than 10^0. |
| pmmccorkell | 0:37123f30e8b2 | 364 | //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 | 365 | |
| pmmccorkell | 0:37123f30e8b2 | 366 | //Heading Gains |
| pmmccorkell | 4:e89234ca9d4e | 367 | status[10]= 0xd1100000 + k_data(heading_Kp); //Kp |
| pmmccorkell | 4:e89234ca9d4e | 368 | status[11]= 0xd1200000 + k_data(heading_Ki); //Ki |
| pmmccorkell | 4:e89234ca9d4e | 369 | status[12]= 0xd1300000 + k_data(heading_Kd); //Kd |
| pmmccorkell | 0:37123f30e8b2 | 370 | |
| pmmccorkell | 0:37123f30e8b2 | 371 | //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi. |
| pmmccorkell | 0:37123f30e8b2 | 372 | int i; |
| pmmccorkell | 0:37123f30e8b2 | 373 | int l=(sizeof(status)/sizeof(uint32_t))-1; |
| pmmccorkell | 0:37123f30e8b2 | 374 | for (i=0; i<=l; i++) { |
| pmmccorkell | 0:37123f30e8b2 | 375 | pc.printf("%x\n", status[i]); |
| pmmccorkell | 0:37123f30e8b2 | 376 | } |
| pmmccorkell | 0:37123f30e8b2 | 377 | //function_timer2=0; |
| pmmccorkell | 0:37123f30e8b2 | 378 | } |
| pmmccorkell | 0:37123f30e8b2 | 379 | |
| pmmccorkell | 0:37123f30e8b2 | 380 | //reset all controller set points and zero out any accumulated integral gains to ESCs |
| pmmccorkell | 0:37123f30e8b2 | 381 | void stop_all_persistent() { |
| pmmccorkell | 0:37123f30e8b2 | 382 | persistent_heading=-1; |
| pmmccorkell | 0:37123f30e8b2 | 383 | persistent_speed=-1; |
| pmmccorkell | 0:37123f30e8b2 | 384 | pid_heading.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 385 | // pid_speed.reset(); |
| pmmccorkell | 0:37123f30e8b2 | 386 | } |
| pmmccorkell | 0:37123f30e8b2 | 387 | |
| pmmccorkell | 0:37123f30e8b2 | 388 | //When bad things are happening. |
| pmmccorkell | 0:37123f30e8b2 | 389 | void EventHorizon() { |
| pmmccorkell | 0:37123f30e8b2 | 390 | event_horizon_flag=1; |
| pmmccorkell | 0:37123f30e8b2 | 391 | stop_all_persistent(); |
| pmmccorkell | 0:37123f30e8b2 | 392 | horizon_count++; |
| pmmccorkell | 0:37123f30e8b2 | 393 | pc.printf("log: EventHorizon called, count: %i\r\n",horizon_count); |
| pmmccorkell | 0:37123f30e8b2 | 394 | //setEvent() method locks out Thruster class set_speed() function |
| pmmccorkell | 0:37123f30e8b2 | 395 | // and sets PWM to 1.5ms. |
| pmmccorkell | 4:e89234ca9d4e | 396 | fwd_port.setEvent(); |
| pmmccorkell | 4:e89234ca9d4e | 397 | fwd_star.setEvent(); |
| pmmccorkell | 4:e89234ca9d4e | 398 | aft_port.setEvent(); |
| pmmccorkell | 4:e89234ca9d4e | 399 | aft_star.setEvent(); |
| pmmccorkell | 0:37123f30e8b2 | 400 | pc.printf("log: Thruster events successfully set\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 401 | //Tells Raspi that Emergency state has been initiated. |
| pmmccorkell | 0:37123f30e8b2 | 402 | ready_prefix=(horizon_prefix+horizon_count); |
| pmmccorkell | 0:37123f30e8b2 | 403 | //Wait some time during which Thruster set_speed() functions are locked out. |
| pmmccorkell | 0:37123f30e8b2 | 404 | for (int i=0; i<200; i++) { |
| pmmccorkell | 0:37123f30e8b2 | 405 | //Resume streaming data to RasPi during timeout period. |
| pmmccorkell | 0:37123f30e8b2 | 406 | az_data(); |
| pmmccorkell | 0:37123f30e8b2 | 407 | wait_ms(wait_main); |
| pmmccorkell | 0:37123f30e8b2 | 408 | } |
| pmmccorkell | 0:37123f30e8b2 | 409 | //Clear emergency situation. |
| pmmccorkell | 4:e89234ca9d4e | 410 | fwd_port.clearEvent(); |
| pmmccorkell | 4:e89234ca9d4e | 411 | fwd_star.clearEvent(); |
| pmmccorkell | 4:e89234ca9d4e | 412 | aft_port.clearEvent(); |
| pmmccorkell | 4:e89234ca9d4e | 413 | aft_star.clearEvent(); |
| pmmccorkell | 0:37123f30e8b2 | 414 | pc.printf("log: Thruster events successfully cleared\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 415 | //Set PWM to 1.5ms after emergency situation. Should have been set to 1.5ms, but double checking. |
| pmmccorkell | 0:37123f30e8b2 | 416 | // For extra precaution. |
| pmmccorkell | 0:37123f30e8b2 | 417 | stop_all_persistent(); |
| pmmccorkell | 0:37123f30e8b2 | 418 | //Lets Raspi know that Mbed is ready for commands again. |
| pmmccorkell | 0:37123f30e8b2 | 419 | ready_prefix=0xffff; |
| pmmccorkell | 0:37123f30e8b2 | 420 | pc.printf("log: ready status reset, mbed may resume\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 421 | event_horizon_flag=0; |
| pmmccorkell | 0:37123f30e8b2 | 422 | } |
| pmmccorkell | 0:37123f30e8b2 | 423 | |
| pmmccorkell | 0:37123f30e8b2 | 424 | //Commands function handles Serial input, checks for correct syntax, and calls appropriate function(s) to execute commands. |
| pmmccorkell | 0:37123f30e8b2 | 425 | int read_serial() { |
| pmmccorkell | 0:37123f30e8b2 | 426 | int i=0; |
| pmmccorkell | 0:37123f30e8b2 | 427 | while (pc.readable()) { |
| pmmccorkell | 0:37123f30e8b2 | 428 | readline[i]=pc.getc(); |
| pmmccorkell | 0:37123f30e8b2 | 429 | pc.printf("log: input read %c at %i\r\n",readline[i],i); |
| pmmccorkell | 0:37123f30e8b2 | 430 | i++; |
| pmmccorkell | 0:37123f30e8b2 | 431 | } |
| pmmccorkell | 0:37123f30e8b2 | 432 | //pc.printf("i: %i\r\n",i); |
| pmmccorkell | 0:37123f30e8b2 | 433 | return i; |
| pmmccorkell | 0:37123f30e8b2 | 434 | } |
| pmmccorkell | 0:37123f30e8b2 | 435 | int look_horizon() { |
| pmmccorkell | 0:37123f30e8b2 | 436 | int returnval=0; |
| pmmccorkell | 0:37123f30e8b2 | 437 | pc.printf("log: THREAD START, horizon\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 438 | //int length=0; |
| pmmccorkell | 0:37123f30e8b2 | 439 | //if (length==4) { |
| pmmccorkell | 0:37123f30e8b2 | 440 | char check_HORIZON[5]="STOP"; |
| pmmccorkell | 0:37123f30e8b2 | 441 | int verified_HORIZON=0; |
| pmmccorkell | 0:37123f30e8b2 | 442 | for (int i=0; i<5; i++) { |
| pmmccorkell | 0:37123f30e8b2 | 443 | if (readline[i]==check_HORIZON[i]) verified_HORIZON++; |
| pmmccorkell | 0:37123f30e8b2 | 444 | } |
| pmmccorkell | 0:37123f30e8b2 | 445 | if (verified_HORIZON==4) { |
| pmmccorkell | 0:37123f30e8b2 | 446 | EventHorizon(); |
| pmmccorkell | 0:37123f30e8b2 | 447 | returnval=1; |
| pmmccorkell | 0:37123f30e8b2 | 448 | } |
| pmmccorkell | 0:37123f30e8b2 | 449 | //} |
| pmmccorkell | 0:37123f30e8b2 | 450 | pc.printf("log: THREAD END, horizon\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 451 | return returnval; |
| pmmccorkell | 0:37123f30e8b2 | 452 | } |
| pmmccorkell | 0:37123f30e8b2 | 453 | |
| pmmccorkell | 4:e89234ca9d4e | 454 | //TODO: for lateral movement |
| pmmccorkell | 4:e89234ca9d4e | 455 | //double offsetController() { |
| pmmccorkell | 4:e89234ca9d4e | 456 | //} |
| pmmccorkell | 0:37123f30e8b2 | 457 | |
| pmmccorkell | 0:37123f30e8b2 | 458 | //Function to go to set heading. |
| pmmccorkell | 0:37123f30e8b2 | 459 | //Timed with DigitalOut on Oscope. |
| pmmccorkell | 0:37123f30e8b2 | 460 | //With no heading set, 28.6us. |
| pmmccorkell | 0:37123f30e8b2 | 461 | //With heading calculations, ~32.8us |
| pmmccorkell | 0:37123f30e8b2 | 462 | //With logging added, ~1.278ms -> serial print statements induce significant delays |
| pmmccorkell | 0:37123f30e8b2 | 463 | double headingController() { |
| pmmccorkell | 0:37123f30e8b2 | 464 | // call_threads_available=0; |
| pmmccorkell | 0:37123f30e8b2 | 465 | double diff=0; |
| pmmccorkell | 0:37123f30e8b2 | 466 | double speed=0; |
| pmmccorkell | 0:37123f30e8b2 | 467 | double dir=1; |
| pmmccorkell | 0:37123f30e8b2 | 468 | double desired_heading=persistent_heading; |
| pmmccorkell | 0:37123f30e8b2 | 469 | double current_heading=heading; |
| pmmccorkell | 0:37123f30e8b2 | 470 | current_heading=(current_heading/16); |
| pmmccorkell | 0:37123f30e8b2 | 471 | if (desired_heading!=-1) { |
| pmmccorkell | 0:37123f30e8b2 | 472 | //Calculate how far to turn in degrees. |
| pmmccorkell | 0:37123f30e8b2 | 473 | diff = abs(desired_heading-current_heading); |
| pmmccorkell | 0:37123f30e8b2 | 474 | //pc.printf("log: diff before if: %f\r\n",diff); |
| pmmccorkell | 0:37123f30e8b2 | 475 | |
| pmmccorkell | 0:37123f30e8b2 | 476 | //Correct for 360-0 edge cases if 'diff'erence is greater than 180. |
| pmmccorkell | 0:37123f30e8b2 | 477 | //Change direction and recalculate for accurate 'tolerance' comparison. |
| pmmccorkell | 0:37123f30e8b2 | 478 | if (diff>180.0) { |
| pmmccorkell | 0:37123f30e8b2 | 479 | //pc.printf("log: diff>180: %f\r\n",diff); |
| pmmccorkell | 0:37123f30e8b2 | 480 | if (desired_heading>180) { |
| pmmccorkell | 0:37123f30e8b2 | 481 | current_heading=current_heading+180; |
| pmmccorkell | 0:37123f30e8b2 | 482 | desired_heading=desired_heading-180; |
| pmmccorkell | 0:37123f30e8b2 | 483 | pc.printf("log: desired>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff); |
| pmmccorkell | 0:37123f30e8b2 | 484 | } |
| pmmccorkell | 4:e89234ca9d4e | 485 | else { //current_heading>180 |
| pmmccorkell | 0:37123f30e8b2 | 486 | current_heading=current_heading-180; |
| pmmccorkell | 0:37123f30e8b2 | 487 | desired_heading=desired_heading+180; |
| pmmccorkell | 4:e89234ca9d4e | 488 | pc.printf("log: current>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff); |
| pmmccorkell | 0:37123f30e8b2 | 489 | } |
| pmmccorkell | 0:37123f30e8b2 | 490 | } |
| pmmccorkell | 4:e89234ca9d4e | 491 | speed = pid_heading.process(desired_heading,current_heading); |
| pmmccorkell | 4:e89234ca9d4e | 492 | // pc.printf("log: des %f, cur %f, pr speed %f\r\n",desired_heading,current_heading,speed); |
| pmmccorkell | 0:37123f30e8b2 | 493 | |
| pmmccorkell | 0:37123f30e8b2 | 494 | //Necessary to overcome 25us deadzone around 1.5ms |
| pmmccorkell | 4:e89234ca9d4e | 495 | if ((abs(speed)<min_thruster_bias) and (abs(diff)>az_tolerance)) { |
| pmmccorkell | 0:37123f30e8b2 | 496 | if (speed<0) speed=(-1*min_thruster_bias); |
| pmmccorkell | 0:37123f30e8b2 | 497 | else speed=min_thruster_bias; |
| pmmccorkell | 0:37123f30e8b2 | 498 | } |
| pmmccorkell | 0:37123f30e8b2 | 499 | } |
| pmmccorkell | 0:37123f30e8b2 | 500 | // call_threads_available=1; |
| pmmccorkell | 0:37123f30e8b2 | 501 | return (speed*dir); |
| pmmccorkell | 0:37123f30e8b2 | 502 | } |
| pmmccorkell | 0:37123f30e8b2 | 503 | |
| pmmccorkell | 4:e89234ca9d4e | 504 | //Controller to maintain the scalar component of velocity vector for starboard front and port aft thrusters. |
| pmmccorkell | 4:e89234ca9d4e | 505 | double cos_speedController() { |
| pmmccorkell | 0:37123f30e8b2 | 506 | // call_threads_available=0; |
| pmmccorkell | 4:e89234ca9d4e | 507 | double offset_factor=0; |
| pmmccorkell | 4:e89234ca9d4e | 508 | if (persistent_offset!=-1) { |
| pmmccorkell | 4:e89234ca9d4e | 509 | offset_factor=(((2*PI_math)/360)*persistent_offset); |
| pmmccorkell | 4:e89234ca9d4e | 510 | } |
| pmmccorkell | 4:e89234ca9d4e | 511 | offset_factor+=(PI_math/4); |
| pmmccorkell | 4:e89234ca9d4e | 512 | double desired_speed=persistent_speed*cos(offset_factor); |
| pmmccorkell | 0:37123f30e8b2 | 513 | desired_speed=(desired_speed/1000); //convert int us to ms |
| pmmccorkell | 0:37123f30e8b2 | 514 | |
| pmmccorkell | 4:e89234ca9d4e | 515 | // call_threads_available=1; |
| pmmccorkell | 4:e89234ca9d4e | 516 | return (desired_speed); |
| pmmccorkell | 4:e89234ca9d4e | 517 | } |
| pmmccorkell | 4:e89234ca9d4e | 518 | |
| pmmccorkell | 4:e89234ca9d4e | 519 | //Controller to maintain the scalar component of velocity vector for port front and starboard aft thrusters. |
| pmmccorkell | 4:e89234ca9d4e | 520 | double sin_speedController() { |
| pmmccorkell | 4:e89234ca9d4e | 521 | // call_threads_available=0; |
| pmmccorkell | 4:e89234ca9d4e | 522 | double offset_factor=0; |
| pmmccorkell | 4:e89234ca9d4e | 523 | if (persistent_offset!=-1) { |
| pmmccorkell | 4:e89234ca9d4e | 524 | offset_factor=(((2*PI_math)/360)*persistent_offset); |
| pmmccorkell | 4:e89234ca9d4e | 525 | } |
| pmmccorkell | 4:e89234ca9d4e | 526 | offset_factor+=(PI_math/4); |
| pmmccorkell | 4:e89234ca9d4e | 527 | double desired_speed=persistent_speed*sin(offset_factor); |
| pmmccorkell | 4:e89234ca9d4e | 528 | desired_speed=(desired_speed/1000); //convert int us to ms |
| pmmccorkell | 0:37123f30e8b2 | 529 | |
| pmmccorkell | 0:37123f30e8b2 | 530 | // call_threads_available=1; |
| pmmccorkell | 0:37123f30e8b2 | 531 | return (desired_speed); |
| pmmccorkell | 0:37123f30e8b2 | 532 | } |
| pmmccorkell | 0:37123f30e8b2 | 533 | |
| pmmccorkell | 0:37123f30e8b2 | 534 | // Make superposition of all Controllers accessing thrusters acting on Az plane. |
| pmmccorkell | 0:37123f30e8b2 | 535 | // Only function that shall write to Az plane thrusters. |
| pmmccorkell | 0:37123f30e8b2 | 536 | // This will also deprecate call_threads() function. |
| pmmccorkell | 0:37123f30e8b2 | 537 | void az_thruster_logic() { |
| pmmccorkell | 0:37123f30e8b2 | 538 | if (manual_mode==0) { |
| pmmccorkell | 0:37123f30e8b2 | 539 | // function_timer3=1; |
| pmmccorkell | 0:37123f30e8b2 | 540 | // logic_available=0; |
| pmmccorkell | 0:37123f30e8b2 | 541 | double heading_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 542 | double sin_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 543 | double cos_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 544 | double fwd_star_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 545 | double fwd_port_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 546 | double aft_star_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 547 | double aft_port_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 548 | if (persistent_speed==-1) { |
| pmmccorkell | 4:e89234ca9d4e | 549 | cos_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 550 | sin_speed=0; |
| pmmccorkell | 4:e89234ca9d4e | 551 | } |
| pmmccorkell | 0:37123f30e8b2 | 552 | if (persistent_heading==-1) heading_speed=0; |
| pmmccorkell | 0:37123f30e8b2 | 553 | if (persistent_heading!=-1) { |
| pmmccorkell | 0:37123f30e8b2 | 554 | heading_speed=headingController(); |
| pmmccorkell | 0:37123f30e8b2 | 555 | } |
| pmmccorkell | 0:37123f30e8b2 | 556 | if (persistent_speed!=-1) { |
| pmmccorkell | 4:e89234ca9d4e | 557 | cos_speed=cos_speedController(); |
| pmmccorkell | 4:e89234ca9d4e | 558 | sin_speed=sin_speedController(); |
| pmmccorkell | 0:37123f30e8b2 | 559 | } |
| pmmccorkell | 0:37123f30e8b2 | 560 | |
| pmmccorkell | 0:37123f30e8b2 | 561 | //Create Superposition of Heading and Speed |
| pmmccorkell | 4:e89234ca9d4e | 562 | fwd_star_speed=(cos_speed - heading_speed); |
| pmmccorkell | 4:e89234ca9d4e | 563 | aft_port_speed=(cos_speed + heading_speed); |
| pmmccorkell | 4:e89234ca9d4e | 564 | fwd_port_speed=(sin_speed + heading_speed); |
| pmmccorkell | 4:e89234ca9d4e | 565 | aft_star_speed=(sin_speed - heading_speed); |
| pmmccorkell | 0:37123f30e8b2 | 566 | |
| pmmccorkell | 0:37123f30e8b2 | 567 | //Error checking to ensure PWM doesn't escape ESC parameters |
| pmmccorkell | 4:e89234ca9d4e | 568 | if (fwd_port_speed<(-1*esc_range)) fwd_port_speed=(-1*esc_range); |
| pmmccorkell | 4:e89234ca9d4e | 569 | if (fwd_star_speed<(-1*esc_range)) fwd_star_speed=(-1*esc_range); |
| pmmccorkell | 4:e89234ca9d4e | 570 | if (aft_star_speed<(-1*esc_range)) aft_star_speed=(-1*esc_range); |
| pmmccorkell | 4:e89234ca9d4e | 571 | if (aft_port_speed<(-1*esc_range)) aft_port_speed=(-1*esc_range); |
| pmmccorkell | 4:e89234ca9d4e | 572 | |
| pmmccorkell | 4:e89234ca9d4e | 573 | if (fwd_port_speed>esc_range) fwd_port_speed=esc_range; |
| pmmccorkell | 4:e89234ca9d4e | 574 | if (fwd_star_speed>esc_range) fwd_star_speed=esc_range; |
| pmmccorkell | 4:e89234ca9d4e | 575 | if (aft_star_speed>esc_range) aft_star_speed=esc_range; |
| pmmccorkell | 4:e89234ca9d4e | 576 | if (aft_port_speed>esc_range) aft_port_speed=esc_range; |
| pmmccorkell | 0:37123f30e8b2 | 577 | |
| pmmccorkell | 4:e89234ca9d4e | 578 | //Write PWM values. |
| pmmccorkell | 4:e89234ca9d4e | 579 | fwd_port.set_speed(fwd_port_speed); |
| pmmccorkell | 4:e89234ca9d4e | 580 | aft_port.set_speed(aft_port_speed); |
| pmmccorkell | 4:e89234ca9d4e | 581 | //pc.printf("log: port %f\r\n",port_speed); |
| pmmccorkell | 4:e89234ca9d4e | 582 | fwd_star.set_speed(fwd_star_speed); |
| pmmccorkell | 4:e89234ca9d4e | 583 | aft_star.set_speed(aft_star_speed); |
| pmmccorkell | 4:e89234ca9d4e | 584 | //pc.printf("log: stbd %f\r\n",starboard_speed); |
| pmmccorkell | 4:e89234ca9d4e | 585 | //logic_available=1; |
| pmmccorkell | 4:e89234ca9d4e | 586 | //function_timer3=0; |
| pmmccorkell | 0:37123f30e8b2 | 587 | } |
| pmmccorkell | 0:37123f30e8b2 | 588 | } |
| pmmccorkell | 0:37123f30e8b2 | 589 | |
| pmmccorkell | 0:37123f30e8b2 | 590 | |
| pmmccorkell | 4:e89234ca9d4e | 591 | //Function to change BNO position. |
| pmmccorkell | 0:37123f30e8b2 | 592 | void switch_pos(int position) { |
| pmmccorkell | 0:37123f30e8b2 | 593 | uint16_t ready_mask=0xfff8; |
| pmmccorkell | 0:37123f30e8b2 | 594 | if (position>=0 and position<8) { |
| pmmccorkell | 0:37123f30e8b2 | 595 | switch (position) { |
| pmmccorkell | 0:37123f30e8b2 | 596 | case 1: |
| pmmccorkell | 0:37123f30e8b2 | 597 | imu.set_mounting_position(MT_P1); |
| pmmccorkell | 0:37123f30e8b2 | 598 | ready_data=((ready_data & ready_mask)+0x001); |
| pmmccorkell | 0:37123f30e8b2 | 599 | break; |
| pmmccorkell | 0:37123f30e8b2 | 600 | case 2: |
| pmmccorkell | 0:37123f30e8b2 | 601 | imu.set_mounting_position(MT_P2); |
| pmmccorkell | 0:37123f30e8b2 | 602 | ready_data=((ready_data & ready_mask)+0x002); |
| pmmccorkell | 0:37123f30e8b2 | 603 | break; |
| pmmccorkell | 0:37123f30e8b2 | 604 | case 3: |
| pmmccorkell | 0:37123f30e8b2 | 605 | imu.set_mounting_position(MT_P3); |
| pmmccorkell | 0:37123f30e8b2 | 606 | ready_data=((ready_data & ready_mask)+0x003); |
| pmmccorkell | 0:37123f30e8b2 | 607 | break; |
| pmmccorkell | 0:37123f30e8b2 | 608 | case 4: |
| pmmccorkell | 0:37123f30e8b2 | 609 | imu.set_mounting_position(MT_P4); |
| pmmccorkell | 0:37123f30e8b2 | 610 | ready_data=((ready_data & ready_mask)+0x004); |
| pmmccorkell | 0:37123f30e8b2 | 611 | break; |
| pmmccorkell | 0:37123f30e8b2 | 612 | case 5: |
| pmmccorkell | 0:37123f30e8b2 | 613 | imu.set_mounting_position(MT_P5); |
| pmmccorkell | 0:37123f30e8b2 | 614 | ready_data=((ready_data & ready_mask)+0x005); |
| pmmccorkell | 0:37123f30e8b2 | 615 | break; |
| pmmccorkell | 0:37123f30e8b2 | 616 | case 6: |
| pmmccorkell | 0:37123f30e8b2 | 617 | imu.set_mounting_position(MT_P6); |
| pmmccorkell | 0:37123f30e8b2 | 618 | ready_data=((ready_data & ready_mask)+0x006); |
| pmmccorkell | 0:37123f30e8b2 | 619 | break; |
| pmmccorkell | 0:37123f30e8b2 | 620 | case 7: |
| pmmccorkell | 0:37123f30e8b2 | 621 | imu.set_mounting_position(MT_P7); |
| pmmccorkell | 0:37123f30e8b2 | 622 | ready_data=((ready_data & ready_mask)+0x007); |
| pmmccorkell | 0:37123f30e8b2 | 623 | break; |
| pmmccorkell | 0:37123f30e8b2 | 624 | case 0: |
| pmmccorkell | 0:37123f30e8b2 | 625 | default: |
| pmmccorkell | 0:37123f30e8b2 | 626 | imu.set_mounting_position(MT_P0); |
| pmmccorkell | 0:37123f30e8b2 | 627 | ready_data=((ready_data & ready_mask)); |
| pmmccorkell | 0:37123f30e8b2 | 628 | break; |
| pmmccorkell | 0:37123f30e8b2 | 629 | } |
| pmmccorkell | 0:37123f30e8b2 | 630 | } |
| pmmccorkell | 0:37123f30e8b2 | 631 | } |
| pmmccorkell | 0:37123f30e8b2 | 632 | |
| pmmccorkell | 4:e89234ca9d4e | 633 | //Function to change BNO Fusion Modes. |
| pmmccorkell | 4:e89234ca9d4e | 634 | void switch_mode(uint8_t new_mode) { |
| pmmccorkell | 4:e89234ca9d4e | 635 | imu.change_fusion_mode(new_mode); |
| pmmccorkell | 4:e89234ca9d4e | 636 | pc.printf("log: change BNO mod to %u\r\n",new_mode); |
| pmmccorkell | 4:e89234ca9d4e | 637 | // int switch_int = (int)new_mode; |
| pmmccorkell | 4:e89234ca9d4e | 638 | // switch (switch_int) { |
| pmmccorkell | 4:e89234ca9d4e | 639 | // case 1: |
| pmmccorkell | 4:e89234ca9d4e | 640 | // imu.change_fusion_mode(MODE_M4G); |
| pmmccorkell | 4:e89234ca9d4e | 641 | // pc.printf("log: change BNO mode to M4G\r\n"); |
| pmmccorkell | 4:e89234ca9d4e | 642 | // break; |
| pmmccorkell | 4:e89234ca9d4e | 643 | // case 2: |
| pmmccorkell | 4:e89234ca9d4e | 644 | // imu.change_fusion_mode(MODE_COMPASS); |
| pmmccorkell | 4:e89234ca9d4e | 645 | // pc.printf("log: change BNO mode to COMPASS\r\n"); |
| pmmccorkell | 4:e89234ca9d4e | 646 | // break; |
| pmmccorkell | 4:e89234ca9d4e | 647 | // case 3: |
| pmmccorkell | 4:e89234ca9d4e | 648 | // imu.change_fusion_mode(MODE_IMU); |
| pmmccorkell | 4:e89234ca9d4e | 649 | // pc.printf("log: change BNO mode to IMU\r\n"); |
| pmmccorkell | 4:e89234ca9d4e | 650 | // break; |
| pmmccorkell | 4:e89234ca9d4e | 651 | // case 12: |
| pmmccorkell | 4:e89234ca9d4e | 652 | // default: |
| pmmccorkell | 4:e89234ca9d4e | 653 | // imu.change_fusion_mode(MODE_NDOF); |
| pmmccorkell | 4:e89234ca9d4e | 654 | // pc.printf("log: change BNO mode to NDOF\r\n"); |
| pmmccorkell | 4:e89234ca9d4e | 655 | // break; |
| pmmccorkell | 4:e89234ca9d4e | 656 | // } |
| pmmccorkell | 0:37123f30e8b2 | 657 | } |
| pmmccorkell | 0:37123f30e8b2 | 658 | |
| pmmccorkell | 0:37123f30e8b2 | 659 | //Function for manual up/down, left/right controls. |
| pmmccorkell | 0:37123f30e8b2 | 660 | //Takes current measurement of relevant sensor and adds/subtracts the "resolution" times "magnitude" |
| pmmccorkell | 0:37123f30e8b2 | 661 | // to new set point, incrementally driving vehicle. |
| pmmccorkell | 0:37123f30e8b2 | 662 | // ie current heading 10deg, function(3,-1), new heading set point of 10+(-1*3) = 7deg |
| pmmccorkell | 0:37123f30e8b2 | 663 | void increment_persistent(int select, int magnitude) { |
| pmmccorkell | 0:37123f30e8b2 | 664 | int heading_resolution=3; //degrees |
| pmmccorkell | 0:37123f30e8b2 | 665 | int speed_resolution=27; //us |
| pmmccorkell | 0:37123f30e8b2 | 666 | switch (select) { |
| pmmccorkell | 0:37123f30e8b2 | 667 | //heading |
| pmmccorkell | 0:37123f30e8b2 | 668 | case 3: |
| pmmccorkell | 0:37123f30e8b2 | 669 | persistent_heading=((heading/16)+(magnitude*heading_resolution)); |
| pmmccorkell | 0:37123f30e8b2 | 670 | pid_heading.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 671 | break; |
| pmmccorkell | 0:37123f30e8b2 | 672 | |
| pmmccorkell | 0:37123f30e8b2 | 673 | //speed |
| pmmccorkell | 0:37123f30e8b2 | 674 | case 4: |
| pmmccorkell | 4:e89234ca9d4e | 675 | persistent_speed=((1000*fwd_star.get_speed())+(magnitude*speed_resolution)); |
| pmmccorkell | 0:37123f30e8b2 | 676 | // pid_speed.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 677 | break; |
| pmmccorkell | 4:e89234ca9d4e | 678 | |
| pmmccorkell | 4:e89234ca9d4e | 679 | //offset |
| pmmccorkell | 4:e89234ca9d4e | 680 | case 5: |
| pmmccorkell | 4:e89234ca9d4e | 681 | persistent_offset=(persistent_offset+(magnitude*heading_resolution)); |
| pmmccorkell | 4:e89234ca9d4e | 682 | break; |
| pmmccorkell | 0:37123f30e8b2 | 683 | } |
| pmmccorkell | 0:37123f30e8b2 | 684 | } |
| pmmccorkell | 0:37123f30e8b2 | 685 | |
| pmmccorkell | 4:e89234ca9d4e | 686 | float k_commands(int value, int power) { |
| pmmccorkell | 4:e89234ca9d4e | 687 | static float pow[10] = { |
| pmmccorkell | 4:e89234ca9d4e | 688 | 0.00001, 0.0001, 0.001, 0.01, 0.1, //[0,4] |
| pmmccorkell | 4:e89234ca9d4e | 689 | 1, 10, 100, 1000, 10000 //[5,9] |
| pmmccorkell | 4:e89234ca9d4e | 690 | }; |
| pmmccorkell | 4:e89234ca9d4e | 691 | float return_val=0; |
| pmmccorkell | 4:e89234ca9d4e | 692 | return_val = value*pow[power]; |
| pmmccorkell | 4:e89234ca9d4e | 693 | pc.printf("log: return %f, pow %lf\r\n",return_val,pow[power]); |
| pmmccorkell | 4:e89234ca9d4e | 694 | return return_val; |
| pmmccorkell | 4:e89234ca9d4e | 695 | } |
| pmmccorkell | 4:e89234ca9d4e | 696 | |
| pmmccorkell | 0:37123f30e8b2 | 697 | //Function intakes serial commands. |
| pmmccorkell | 0:37123f30e8b2 | 698 | void commands() { |
| pmmccorkell | 0:37123f30e8b2 | 699 | //pc.printf("log: commands called\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 700 | int length=0; |
| pmmccorkell | 0:37123f30e8b2 | 701 | length=read_serial(); |
| pmmccorkell | 0:37123f30e8b2 | 702 | if (length==4) { |
| pmmccorkell | 0:37123f30e8b2 | 703 | look_horizon(); |
| pmmccorkell | 0:37123f30e8b2 | 704 | } |
| pmmccorkell | 0:37123f30e8b2 | 705 | if (length==7) { |
| pmmccorkell | 0:37123f30e8b2 | 706 | char input[10]; |
| pmmccorkell | 0:37123f30e8b2 | 707 | for (int i=0; i<10; i++) { |
| pmmccorkell | 0:37123f30e8b2 | 708 | input[i]=readline[i]; |
| pmmccorkell | 0:37123f30e8b2 | 709 | //pc.printf("Command thread: read %c at %i\r\n",readline[i],i); |
| pmmccorkell | 0:37123f30e8b2 | 710 | } |
| pmmccorkell | 0:37123f30e8b2 | 711 | //check_pos char array used to filter Position commands. |
| pmmccorkell | 0:37123f30e8b2 | 712 | char check_pos[5]="pos:"; |
| pmmccorkell | 4:e89234ca9d4e | 713 | //check_mod char array used to filter Fusion modes on BNO. |
| pmmccorkell | 4:e89234ca9d4e | 714 | char check_mod[5]="mod:"; |
| pmmccorkell | 4:e89234ca9d4e | 715 | //Heading commands. |
| pmmccorkell | 0:37123f30e8b2 | 716 | char check_hea[5]="hea:"; |
| pmmccorkell | 4:e89234ca9d4e | 717 | //Speed commands. |
| pmmccorkell | 4:e89234ca9d4e | 718 | char check_vel[5]="vel:"; |
| pmmccorkell | 4:e89234ca9d4e | 719 | //Offset commands. |
| pmmccorkell | 4:e89234ca9d4e | 720 | char check_off[5]="off:"; |
| pmmccorkell | 4:e89234ca9d4e | 721 | //Stop all commands. |
| pmmccorkell | 0:37123f30e8b2 | 722 | char check_sto[5]="sto:"; |
| pmmccorkell | 4:e89234ca9d4e | 723 | //Reset Mbed command. |
| pmmccorkell | 0:37123f30e8b2 | 724 | char check_res[5]="res:"; |
| pmmccorkell | 0:37123f30e8b2 | 725 | |
| pmmccorkell | 4:e89234ca9d4e | 726 | //Heading PID gains |
| pmmccorkell | 4:e89234ca9d4e | 727 | char check_hkp[5]="hkp:"; |
| pmmccorkell | 4:e89234ca9d4e | 728 | char check_hki[5]="hki:"; |
| pmmccorkell | 4:e89234ca9d4e | 729 | char check_hkd[5]="hkd:"; |
| pmmccorkell | 4:e89234ca9d4e | 730 | |
| pmmccorkell | 0:37123f30e8b2 | 731 | //While loop reads Serial input into input buffer. |
| pmmccorkell | 0:37123f30e8b2 | 732 | |
| pmmccorkell | 0:37123f30e8b2 | 733 | //Only continue if input buffer has 7 entries, otherwise ignore input buffer. |
| pmmccorkell | 0:37123f30e8b2 | 734 | //All commands from RasPi shall come in a format of 7 characters "abc:xyz" |
| pmmccorkell | 0:37123f30e8b2 | 735 | // where 'abc' is an identifying string and 'xyz' is some data/information. |
| pmmccorkell | 0:37123f30e8b2 | 736 | // if (i==7) { |
| pmmccorkell | 0:37123f30e8b2 | 737 | //Instantiate counters that will be used to verify known commands. |
| pmmccorkell | 0:37123f30e8b2 | 738 | int verified_pos=0; |
| pmmccorkell | 4:e89234ca9d4e | 739 | int verified_mod=0; |
| pmmccorkell | 0:37123f30e8b2 | 740 | int verified_hea=0; |
| pmmccorkell | 0:37123f30e8b2 | 741 | int verified_sto=0; |
| pmmccorkell | 0:37123f30e8b2 | 742 | int verified_res=0; |
| pmmccorkell | 0:37123f30e8b2 | 743 | int verified_vel=0; |
| pmmccorkell | 4:e89234ca9d4e | 744 | int verified_off=0; |
| pmmccorkell | 4:e89234ca9d4e | 745 | |
| pmmccorkell | 4:e89234ca9d4e | 746 | int verified_hkp=0; |
| pmmccorkell | 4:e89234ca9d4e | 747 | int verified_hki=0; |
| pmmccorkell | 4:e89234ca9d4e | 748 | int verified_hkd=0; |
| pmmccorkell | 4:e89234ca9d4e | 749 | |
| pmmccorkell | 0:37123f30e8b2 | 750 | //While loop checks first 4 characters of input buffer and attempts to match |
| pmmccorkell | 0:37123f30e8b2 | 751 | // against known commands. |
| pmmccorkell | 0:37123f30e8b2 | 752 | for (int q=0; q<3; q++) { |
| pmmccorkell | 0:37123f30e8b2 | 753 | //Increment verified_pos if a match is found between Serial input buffer |
| pmmccorkell | 0:37123f30e8b2 | 754 | // and Position command format. |
| pmmccorkell | 0:37123f30e8b2 | 755 | if (input[q]==check_pos[q]) { |
| pmmccorkell | 0:37123f30e8b2 | 756 | verified_pos++; |
| pmmccorkell | 4:e89234ca9d4e | 757 | //pc.printf("pos %c at %i\r\n",input[q],q); |
| pmmccorkell | 4:e89234ca9d4e | 758 | } |
| pmmccorkell | 4:e89234ca9d4e | 759 | //Increment verified_mod if a match is found between Serial input buffer |
| pmmccorkell | 4:e89234ca9d4e | 760 | // and Fusion Modde command format. |
| pmmccorkell | 4:e89234ca9d4e | 761 | if (input[q]==check_mod[q]) { |
| pmmccorkell | 4:e89234ca9d4e | 762 | verified_mod++; |
| pmmccorkell | 4:e89234ca9d4e | 763 | //pc.printf("mod %c at %i\r\n",inputtt[q],q); |
| pmmccorkell | 0:37123f30e8b2 | 764 | } |
| pmmccorkell | 0:37123f30e8b2 | 765 | //Increment verified_hea if a match is found between Serial input buffer |
| pmmccorkell | 0:37123f30e8b2 | 766 | // and Heading command format. |
| pmmccorkell | 0:37123f30e8b2 | 767 | if (input[q]==check_hea[q]) { |
| pmmccorkell | 0:37123f30e8b2 | 768 | //pc.printf("hea %c at %i\r\n",input[q],q); |
| pmmccorkell | 0:37123f30e8b2 | 769 | verified_hea++; |
| pmmccorkell | 0:37123f30e8b2 | 770 | } |
| pmmccorkell | 0:37123f30e8b2 | 771 | if (input[q]==check_sto[q]) { |
| pmmccorkell | 0:37123f30e8b2 | 772 | //pc.printf("sto %c at %i\r\n",input[q],q); |
| pmmccorkell | 0:37123f30e8b2 | 773 | verified_sto++; |
| pmmccorkell | 0:37123f30e8b2 | 774 | } |
| pmmccorkell | 0:37123f30e8b2 | 775 | if (input[q]==check_res[q]) { |
| pmmccorkell | 0:37123f30e8b2 | 776 | //pc.printf("res %c at %i\r\n",input[q],q); |
| pmmccorkell | 0:37123f30e8b2 | 777 | verified_res++; |
| pmmccorkell | 0:37123f30e8b2 | 778 | } |
| pmmccorkell | 0:37123f30e8b2 | 779 | if (input[q]==check_vel[q]) { |
| pmmccorkell | 0:37123f30e8b2 | 780 | verified_vel++; |
| pmmccorkell | 0:37123f30e8b2 | 781 | //pc.printf("vel %c at %i\r\n",input[q],q); |
| pmmccorkell | 0:37123f30e8b2 | 782 | } |
| pmmccorkell | 4:e89234ca9d4e | 783 | if (input[q]==check_off[q]) { |
| pmmccorkell | 4:e89234ca9d4e | 784 | verified_off++; |
| pmmccorkell | 4:e89234ca9d4e | 785 | } |
| pmmccorkell | 4:e89234ca9d4e | 786 | if (input[q]==check_hkp[q]) { |
| pmmccorkell | 4:e89234ca9d4e | 787 | verified_hkp++; |
| pmmccorkell | 4:e89234ca9d4e | 788 | } |
| pmmccorkell | 4:e89234ca9d4e | 789 | if (input[q]==check_hki[q]) { |
| pmmccorkell | 4:e89234ca9d4e | 790 | verified_hki++; |
| pmmccorkell | 4:e89234ca9d4e | 791 | } |
| pmmccorkell | 4:e89234ca9d4e | 792 | if (input[q]==check_hkd[q]) { |
| pmmccorkell | 4:e89234ca9d4e | 793 | verified_hkd++; |
| pmmccorkell | 4:e89234ca9d4e | 794 | } |
| pmmccorkell | 0:37123f30e8b2 | 795 | } |
| pmmccorkell | 0:37123f30e8b2 | 796 | |
| pmmccorkell | 0:37123f30e8b2 | 797 | //If first 4 characters from Serial input buffer match Position command format, |
| pmmccorkell | 0:37123f30e8b2 | 798 | // execute "switch_pos" function. |
| pmmccorkell | 0:37123f30e8b2 | 799 | if (verified_pos==3) { |
| pmmccorkell | 0:37123f30e8b2 | 800 | int change=(input[6]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 801 | switch_pos(change); |
| pmmccorkell | 0:37123f30e8b2 | 802 | } |
| pmmccorkell | 4:e89234ca9d4e | 803 | if (verified_mod==3) { |
| pmmccorkell | 4:e89234ca9d4e | 804 | uint8_t mode=(input[6]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 805 | if ((mode>0x0) and (mode<0x0d)) { |
| pmmccorkell | 4:e89234ca9d4e | 806 | switch_mode(mode); |
| pmmccorkell | 4:e89234ca9d4e | 807 | } |
| pmmccorkell | 4:e89234ca9d4e | 808 | } |
| pmmccorkell | 0:37123f30e8b2 | 809 | if (verified_sto==3) { |
| pmmccorkell | 0:37123f30e8b2 | 810 | //pc.printf("log: stop command received\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 811 | stop_all_persistent(); |
| pmmccorkell | 0:37123f30e8b2 | 812 | //pc.printf("log: stop command executed\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 813 | } |
| pmmccorkell | 0:37123f30e8b2 | 814 | //If first 4 characters from Serial input buffer match Heading command format, |
| pmmccorkell | 0:37123f30e8b2 | 815 | // execute "motors" function. |
| pmmccorkell | 0:37123f30e8b2 | 816 | if (verified_hea==3) { |
| pmmccorkell | 0:37123f30e8b2 | 817 | //Correct for ascii '0', and reform 3digit decimal number |
| pmmccorkell | 0:37123f30e8b2 | 818 | int hea100=(input[4]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 819 | int hea10=(input[5]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 820 | int hea1=(input[6]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 821 | int hea=(hea100*100)+(hea10*10)+(hea1*1); |
| pmmccorkell | 0:37123f30e8b2 | 822 | pc.printf("log: heading rx: %i\r\n",hea); |
| pmmccorkell | 0:37123f30e8b2 | 823 | if (hea==999) { |
| pmmccorkell | 0:37123f30e8b2 | 824 | persistent_heading=-1; |
| pmmccorkell | 0:37123f30e8b2 | 825 | pid_heading.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 826 | } |
| pmmccorkell | 0:37123f30e8b2 | 827 | if ((hea>=831) and (hea<=837)) { |
| pmmccorkell | 0:37123f30e8b2 | 828 | increment_persistent(hea10,(hea1-4)); |
| pmmccorkell | 0:37123f30e8b2 | 829 | } |
| pmmccorkell | 0:37123f30e8b2 | 830 | if ((hea>=0) and (hea<=360)) { |
| pmmccorkell | 0:37123f30e8b2 | 831 | pid_heading.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 832 | if (event_horizon_flag==0) persistent_heading=hea; |
| pmmccorkell | 0:37123f30e8b2 | 833 | } |
| pmmccorkell | 0:37123f30e8b2 | 834 | } |
| pmmccorkell | 0:37123f30e8b2 | 835 | if (verified_res==3) { |
| pmmccorkell | 0:37123f30e8b2 | 836 | pc.printf("log: Reset mbed received. See you on the other side.\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 837 | //Mbed reset command. |
| pmmccorkell | 0:37123f30e8b2 | 838 | NVIC_SystemReset(); |
| pmmccorkell | 0:37123f30e8b2 | 839 | //If this print statement is ever executed, reset didn't happen. |
| pmmccorkell | 0:37123f30e8b2 | 840 | pc.printf("log: Reset failed. The show goes on.\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 841 | } |
| pmmccorkell | 0:37123f30e8b2 | 842 | // Forward/Reverse speed commands. |
| pmmccorkell | 0:37123f30e8b2 | 843 | if (verified_vel==3) { |
| pmmccorkell | 0:37123f30e8b2 | 844 | int vel100=(input[4]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 845 | int vel10=(input[5]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 846 | int vel1=(input[6]-'0'); |
| pmmccorkell | 0:37123f30e8b2 | 847 | int vel=(vel100*100)+(vel10*10)+(vel1*1); |
| pmmccorkell | 0:37123f30e8b2 | 848 | pc.printf("log: vel rx: %i\r\n",vel); |
| pmmccorkell | 0:37123f30e8b2 | 849 | //999 to stop speed controller. |
| pmmccorkell | 0:37123f30e8b2 | 850 | if (vel==999) { |
| pmmccorkell | 0:37123f30e8b2 | 851 | persistent_speed=-1; |
| pmmccorkell | 0:37123f30e8b2 | 852 | } |
| pmmccorkell | 0:37123f30e8b2 | 853 | if ((vel>=841) and (vel<=847)) { |
| pmmccorkell | 0:37123f30e8b2 | 854 | increment_persistent(vel10,(vel1-4)); |
| pmmccorkell | 0:37123f30e8b2 | 855 | } |
| pmmccorkell | 0:37123f30e8b2 | 856 | if ((vel>=0) and (vel<=800)) { |
| pmmccorkell | 0:37123f30e8b2 | 857 | // pid_speed.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 858 | vel=(vel-400); |
| pmmccorkell | 0:37123f30e8b2 | 859 | if (event_horizon_flag==0) persistent_speed=vel; |
| pmmccorkell | 0:37123f30e8b2 | 860 | } |
| pmmccorkell | 0:37123f30e8b2 | 861 | } |
| pmmccorkell | 4:e89234ca9d4e | 862 | |
| pmmccorkell | 4:e89234ca9d4e | 863 | //Heading offset commands. |
| pmmccorkell | 4:e89234ca9d4e | 864 | if (verified_off==3) { |
| pmmccorkell | 4:e89234ca9d4e | 865 | int off100=(input[4]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 866 | int off10=(input[5]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 867 | int off1=(input[6]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 868 | int off=(off100*100)+(off10*10)+(off1*1); |
| pmmccorkell | 4:e89234ca9d4e | 869 | pc.printf("log: off rx: %i\r\n",off); |
| pmmccorkell | 4:e89234ca9d4e | 870 | //999 to reset heading offset to 0. |
| pmmccorkell | 4:e89234ca9d4e | 871 | if (off==999) { |
| pmmccorkell | 4:e89234ca9d4e | 872 | persistent_offset=-1; |
| pmmccorkell | 4:e89234ca9d4e | 873 | } |
| pmmccorkell | 4:e89234ca9d4e | 874 | if ((off>=851) and (off<=857)) { |
| pmmccorkell | 4:e89234ca9d4e | 875 | increment_persistent(off10,(off1-4)); |
| pmmccorkell | 4:e89234ca9d4e | 876 | } |
| pmmccorkell | 4:e89234ca9d4e | 877 | if ((off>=0) and (off<=360)) { |
| pmmccorkell | 4:e89234ca9d4e | 878 | if (event_horizon_flag==0) persistent_offset=off; |
| pmmccorkell | 4:e89234ca9d4e | 879 | } |
| pmmccorkell | 4:e89234ca9d4e | 880 | } |
| pmmccorkell | 4:e89234ca9d4e | 881 | |
| pmmccorkell | 4:e89234ca9d4e | 882 | int reset_h=0; |
| pmmccorkell | 4:e89234ca9d4e | 883 | if (verified_hkp==3) { |
| pmmccorkell | 4:e89234ca9d4e | 884 | float kval=((input[4]-'0')*10)+input[5]-'0'; |
| pmmccorkell | 4:e89234ca9d4e | 885 | heading_Kp=k_commands(kval,input[6]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 886 | reset_h=1; |
| pmmccorkell | 4:e89234ca9d4e | 887 | } |
| pmmccorkell | 4:e89234ca9d4e | 888 | if (verified_hki==3) { |
| pmmccorkell | 4:e89234ca9d4e | 889 | float kval=((input[4]-'0')*10)+input[5]-'0'; |
| pmmccorkell | 4:e89234ca9d4e | 890 | heading_Ki=k_commands(kval,input[6]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 891 | reset_h=1; |
| pmmccorkell | 4:e89234ca9d4e | 892 | } |
| pmmccorkell | 4:e89234ca9d4e | 893 | if (verified_hkd==3) { |
| pmmccorkell | 4:e89234ca9d4e | 894 | float kval=((input[4]-'0')*10)+input[5]-'0'; |
| pmmccorkell | 4:e89234ca9d4e | 895 | heading_Kd=k_commands(kval,input[6]-'0'); |
| pmmccorkell | 4:e89234ca9d4e | 896 | reset_h=1; |
| pmmccorkell | 4:e89234ca9d4e | 897 | } |
| pmmccorkell | 4:e89234ca9d4e | 898 | if (reset_h) { |
| pmmccorkell | 4:e89234ca9d4e | 899 | pid_heading.set_K(heading_Kp,heading_Ki,heading_Kd); |
| pmmccorkell | 4:e89234ca9d4e | 900 | pc.printf("log: update Heading gains Kp %f, Ki %f, Kd %f\r\n",heading_Kp, heading_Ki, heading_Kd); |
| pmmccorkell | 0:37123f30e8b2 | 901 | } |
| pmmccorkell | 0:37123f30e8b2 | 902 | } |
| pmmccorkell | 0:37123f30e8b2 | 903 | } |
| pmmccorkell | 0:37123f30e8b2 | 904 | |
| pmmccorkell | 0:37123f30e8b2 | 905 | //Initialize controllers associated with Azimuth, speed and heading. |
| pmmccorkell | 0:37123f30e8b2 | 906 | void init_AzController(void){ |
| pmmccorkell | 0:37123f30e8b2 | 907 | // Zero out PID values. |
| pmmccorkell | 0:37123f30e8b2 | 908 | // pid_speed.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 909 | pid_heading.clear(); |
| pmmccorkell | 0:37123f30e8b2 | 910 | |
| pmmccorkell | 0:37123f30e8b2 | 911 | // run Az controller as set in globals, in ms |
| pmmccorkell | 0:37123f30e8b2 | 912 | tickerAzThrusters.attach(&az_thruster_logic, ticker_rate); |
| pmmccorkell | 0:37123f30e8b2 | 913 | } |
| pmmccorkell | 0:37123f30e8b2 | 914 | |
| pmmccorkell | 0:37123f30e8b2 | 915 | int main() { |
| pmmccorkell | 0:37123f30e8b2 | 916 | //engage plaidspeed |
| pmmccorkell | 0:37123f30e8b2 | 917 | pc.baud(baudrate); |
| pmmccorkell | 0:37123f30e8b2 | 918 | |
| pmmccorkell | 0:37123f30e8b2 | 919 | //Uncomment to derive timing using Oscope. |
| pmmccorkell | 0:37123f30e8b2 | 920 | //function_timer=0; |
| pmmccorkell | 0:37123f30e8b2 | 921 | //function_timer2=0; |
| pmmccorkell | 0:37123f30e8b2 | 922 | //function_timer3=0; |
| pmmccorkell | 0:37123f30e8b2 | 923 | |
| pmmccorkell | 0:37123f30e8b2 | 924 | //If not ready, reset BNO. |
| pmmccorkell | 0:37123f30e8b2 | 925 | while (imu.chip_ready() == 0) { |
| pmmccorkell | 0:37123f30e8b2 | 926 | do { |
| pmmccorkell | 0:37123f30e8b2 | 927 | pc.printf("resetting BNO\r\n"); |
| pmmccorkell | 0:37123f30e8b2 | 928 | pwr_on=0; |
| pmmccorkell | 0:37123f30e8b2 | 929 | wait_ms(100); |
| pmmccorkell | 0:37123f30e8b2 | 930 | pwr_on=1; |
| pmmccorkell | 0:37123f30e8b2 | 931 | wait_ms(50); |
| pmmccorkell | 0:37123f30e8b2 | 932 | } while(imu.reset()); |
| pmmccorkell | 0:37123f30e8b2 | 933 | } |
| pmmccorkell | 0:37123f30e8b2 | 934 | wait_ms(20); |
| pmmccorkell | 0:37123f30e8b2 | 935 | |
| pmmccorkell | 0:37123f30e8b2 | 936 | //If BNO is ready, set ready status indication |
| pmmccorkell | 0:37123f30e8b2 | 937 | if (imu.chip_ready()) { |
| pmmccorkell | 0:37123f30e8b2 | 938 | ready_prefix=0xffff; |
| pmmccorkell | 0:37123f30e8b2 | 939 | } |
| pmmccorkell | 4:e89234ca9d4e | 940 | switch_pos(0); //BNO default position 1 |
| pmmccorkell | 4:e89234ca9d4e | 941 | switch_mode(0x08); //0x08 MODE_IMU |
| pmmccorkell | 0:37123f30e8b2 | 942 | |
| pmmccorkell | 0:37123f30e8b2 | 943 | init_AzController(); |
| pmmccorkell | 0:37123f30e8b2 | 944 | |
| pmmccorkell | 0:37123f30e8b2 | 945 | //Look for serial input commands and send to 'commands' function. |
| pmmccorkell | 0:37123f30e8b2 | 946 | //If no serial input commands, stream data. |
| pmmccorkell | 0:37123f30e8b2 | 947 | while(1) { |
| pmmccorkell | 0:37123f30e8b2 | 948 | if (pc.readable()) { |
| pmmccorkell | 0:37123f30e8b2 | 949 | // command_available=0; |
| pmmccorkell | 0:37123f30e8b2 | 950 | commands(); |
| pmmccorkell | 0:37123f30e8b2 | 951 | //function_timer4=1; |
| pmmccorkell | 0:37123f30e8b2 | 952 | // command_available=1; |
| pmmccorkell | 0:37123f30e8b2 | 953 | } |
| pmmccorkell | 0:37123f30e8b2 | 954 | else { |
| pmmccorkell | 0:37123f30e8b2 | 955 | az_data(); |
| pmmccorkell | 0:37123f30e8b2 | 956 | } |
| pmmccorkell | 0:37123f30e8b2 | 957 | wait_ms(wait_main/2); |
| pmmccorkell | 0:37123f30e8b2 | 958 | //function_timer4=0; |
| pmmccorkell | 0:37123f30e8b2 | 959 | wait_ms(wait_main/2); |
| pmmccorkell | 0:37123f30e8b2 | 960 | } |
| pmmccorkell | 0:37123f30e8b2 | 961 | } |
| pmmccorkell | 0:37123f30e8b2 | 962 |