AUV_PID.cpp

00001 #include "mbed.h"
00002 #include "BNO055.h"     //imu
00003 #include "PID.h"
00004 #define PI_math           3.14159265358979323846
00005 
00006 //Setup USB Serial
00007 Serial pc(USBTX, USBRX);
00008 int baudrate = 115200;
00009 
00010 //Setup BNO055 and MS5837 over I2C
00011 I2C i2c(p28,p27);
00012 DigitalOut pwr_on(p30);
00013 BNO055 imu(i2c,p8);
00014 
00015 int wait_main=20;       //ms to wait in main, and associated, loops
00016 
00017 /*
00018 // IO pins used with oscope to observe and gather timing data of the program itself
00019 DigitalOut function_timer(p5);      //el logic + depth and pitch controllers
00020 DigitalOut function_timer2(p6);     //sensor update and data stream
00021 DigitalOut function_timer3(p7);     //az logic + heading and speed controllers
00022 DigitalOut function_timer4(p8);     //high edge when a command is read and completed execution. 
00023                                         //Pair with RasPi GPIO and use Oscope to time how long it 
00024                                         //takes to send, receive, and execute commands.
00025 */
00026 
00027 DigitalIn leak_detect(p11);
00028 DigitalOut leak_light(LED1);
00029 
00030 // ESC specs data
00031 double esc_freq=400;     //Standard servo rate -> 400Hz
00032 double base_pw=1.5;     //ms
00033 double null_pw=0.0;
00034 double esc_period=(1000/esc_freq);  //ms
00035 double esc_range_spec=.4;   // 400ms, [1.1,1.9]ms pw per data sheet
00036 double esc_range_scale=1.0; //only use x% of full range
00037 double esc_range=esc_range_spec*esc_range_scale;   
00038 double min_thruster_bias=0.028;  //deadzone around 1.5ms where ESC will not operate
00039 double pw_tolerance=0.00001;
00040 
00041 volatile float ticker_rate= 0.02;   //Time between ticker calls in seconds.
00042 float target_time=5;    //Ideal timeframe to reach desired heading, depth, pitch using only Kp.
00043 float target_time_multiple=2;   //Ki will max out PWM in "factor * target_time".
00044 
00045 //
00046 //Azimuth controllers
00047 //
00048 Ticker tickerAzThrusters;
00049 //degrees of acceptable heading tolerance
00050 double az_tolerance=2;
00051 // PID gains for heading
00052 volatile float heading_Kp = 0.0003333f;  //0.0008333f;
00053 volatile float heading_Ki =0.0f;
00054 volatile float heading_Kd =0.0f;        
00055 PID pid_heading(heading_Kp,heading_Ki,heading_Kd,ticker_rate,az_tolerance);
00056 // PID gains for speed
00057 // volatile float speed_Kp=1.0f;
00058 // volatile float speed_Ki=0.0f;
00059 // volatile float speed_Kd=0.0;
00060 // PID pid_speed(speed_Kp, speed_Ki, speed_Kd, ticker_rate, speed_tolerance);
00061 
00062 //instantiate globals for sensor updates and data stream
00063 uint16_t ready_prefix = 0x0000;
00064 uint16_t horizon_prefix=0xff00;
00065 uint16_t ready_data = 0x0000;
00066 uint16_t heading = 0xffff;
00067 uint16_t pitch = 0xffff;
00068 char readline[100];
00069 
00070 //instantiate globals for flags and function indication
00071 int command_available=1;
00072 int call_threads_available=1;
00073 int logic_available=1;
00074 int manual_mode=0;
00075 int zero_set=0;
00076 int horizon_count=0;
00077 int event_horizon_flag=0;
00078 
00079 //instantiate goto position globals
00080 //-1 indicates no setting
00081 int persistent_heading=-1;
00082 int persistent_speed=-1;
00083 int persistent_offset=-1;
00084 
00085 BNO055_ID_INF_TypeDef       bno055_id_inf;
00086 BNO055_EULER_TypeDef        euler_angles;
00087 BNO055_QUATERNION_TypeDef   quaternion;
00088 //BNO055_LIN_ACC_TypeDef      linear_acc;
00089 //BNO055_GRAVITY_TypeDef      gravity;
00090 //BNO055_TEMPERATURE_TypeDef  chip_temp;
00091 
00092 typedef struct {
00093     uint16_t h;
00094     uint16_t r;
00095     uint16_t p;
00096 } CONVERTED_QUATERNION_TypeDef;
00097 
00098 CONVERTED_QUATERNION_TypeDef    conversion;
00099 
00100 //-----THRUSTER CLASS BEGIN-----//
00101 //Thruster class to instantiate individual thrusters.
00102 class Thruster {
00103     public:
00104         Thruster(PinName pin, float dir);
00105         void setEvent();
00106         void clearEvent();
00107         int available();
00108         void set_period(double thruster_time);
00109         void set_pw(double thruster_pw);
00110         double get_pw();
00111         double get_speed();
00112         uint32_t thruster_data();
00113         void set_speed(double pntr);
00114     protected:
00115         PwmOut _pwm;
00116         PinName _pin;
00117         float _d;
00118         int _lock;
00119         int _available;
00120         double _base_pw, _period;
00121 };
00122 
00123 //Instantiation accepts PWM pin and direction
00124 //Direction is -1 or 1. 1 for normal, -1 if blade reversed.
00125 Thruster::Thruster(PinName pin, float dir) : _pwm(pin), _d(dir) {
00126     _lock=0;
00127     _available=1;
00128     _pin=pin;
00129     _base_pw=1.5;
00130     set_pw(_base_pw);
00131     _period=2.5;
00132     set_period(_period);
00133     //pc.printf("Thruster: %f\r\n",d);
00134 }
00135 
00136 //Sets Event for Emergency Stop and sets lockout to set_speed() function.
00137 void Thruster::setEvent() {
00138     _lock=1;
00139     set_pw(_base_pw);
00140 }
00141 
00142 //Clears Event for Emergency Stop of thruster and removes lockout from set_speed() function.
00143 void Thruster::clearEvent() {
00144     _lock=0;
00145 }
00146 
00147 //Returns whether set_speed() function is available, or currently in use.
00148 int Thruster::available() {
00149     return _available;
00150 }
00151 
00152 //Set PWM period in ms.
00153 void Thruster::set_period(double thruster_time) {
00154     _period=thruster_time;
00155     _pwm.period(_period/1000);
00156 }
00157 
00158 //Set PWM pulsewidth in ms
00159 void Thruster::set_pw(double thruster_pw) {
00160     double s_pw=(thruster_pw/1000);
00161     pc.printf("log: set_pw: %f\r\n",s_pw);
00162     _pwm.pulsewidth(s_pw);
00163 }
00164 
00165 //Returns PWM pulsewidth in ms.
00166 double Thruster::get_pw() {
00167     //read duty cycle times period
00168     double g_pw = (_pwm.read()*_period);
00169     //pc.printf(" get_pw: %f, ",g_pw);
00170     return g_pw;
00171 }
00172 
00173 //Returns PWM output relative to 1.5ms.
00174 double Thruster::get_speed() {
00175     double g_speed = (get_pw()-_base_pw);
00176     //pc.printf("get_speed: %f, ",g_speed);
00177     return g_speed;
00178 }
00179 
00180 //formats PWM as an 2 uint16_t joined to make uint32_t for serial data streaming
00181 //MSB uint16_t indicates direction, 0 for positive, 1 for negative.
00182 //LSB uint16_t is 10ms resolution of PWM 
00183 uint32_t Thruster::thruster_data() {
00184     double speed=get_speed();
00185     uint32_t dir=0x0;
00186     uint32_t data=0x0;
00187     if (speed<0) dir =0x00010000;
00188     data=static_cast<unsigned int>(abs(int(speed*100000)));
00189     data=data+dir;
00190     return data;
00191 }
00192 
00193 //Accepts adjustment to pw [-500,500] ms that is added to 1.5ms
00194 void Thruster::set_speed(double speed_pw) {
00195     if (_lock==1) {
00196         set_pw(_base_pw);
00197     }
00198     else {
00199         double tolerance_pw=0.001;
00200         double target_pw=(_d*speed_pw)+_base_pw;
00201         double current_pw=get_pw();
00202         double diff_pw=abs(target_pw-current_pw);
00203         if (diff_pw>tolerance_pw) set_pw(target_pw);
00204     }
00205 }
00206 //-----THRUSTER CLASS END-----//
00207 
00208 // Instantiate thrusters.
00209 Thruster fwd_port(p21,1);
00210 Thruster fwd_star(p22,-1);
00211 //Thruster steadystate(p23,1); //for test purposes, to keep ESC from making LOUD NOISES
00212 Thruster aft_port(p24,-1);
00213 Thruster aft_star(p25,1);
00214 
00215 //Function to check for water leak. Open is good, short is bad.
00216 void leak_data() {
00217     leak_detect.read();
00218     if (leak_detect==1) {
00219         ready_data=(ready_data | 0x0800);
00220     }
00221     leak_light=leak_detect;
00222 }
00223 
00224 //Function to format Gain values into 3 significant digits and exponent value.
00225 // ie 0x abc * 10^(d-9) -> 0x abcd
00226 uint16_t k_data(float k_val) {
00227     
00228     // Static lookup table for 10^11 to 10^-4.
00229     // Arrived at for range to get 3 significant digits for values from 10^-9 to 10^6, respectively.
00230     static double pow10[16] = {
00231         100000000000, 10000000000, 1000000000, 100000000, //[0,4]
00232         10000000, 1000000, 100000, 10000, //[5,8]
00233         1000, 100, 10, 1, //[9,12]
00234         0.1, 0.01, 0.001, 0.0001, //[13,16]
00235     };
00236     
00237     // Find where significant digits start and get the exponent value.
00238     int exponent_value = floor(log10(k_val));
00239 
00240     // Form scalar value of 3 significant digits.
00241     int sig_val = floor((k_val*pow10[exponent_value+9])+0.5);
00242 
00243     // Shift exponent so that 10^-9 starts at 0.
00244     exponent_value+=9;
00245 
00246     // Move Scalar left 1 full hex, and append exponent to the end.
00247     uint16_t return_val=(sig_val<<4)+(exponent_value);
00248 
00249     //Return the formed hex value.
00250     return return_val;
00251 }
00252 
00253 
00254 //Convert Quaternion to Euler angles
00255 //https://forums.adafruit.com/viewtopic.php?f=19&t=95266
00256 void q_to_E(double qw, double qx, double qy, double qz, CONVERTED_QUATERNION_TypeDef *conv) {
00257     double qnorm = 1 / sqrt((double)(qw*qw + qx*qx + qy*qy + qz*qz));  // normalize the quaternion
00258     qw *= qnorm;
00259     qx *= qnorm;
00260     qy *= qnorm;
00261     qz *= qnorm;
00262     double factor = 180.0/PI_math;
00263     double roll_conversion = factor * atan2((double)(qw*qx + qy*qz), (double)(0.5 - qx*qx - qy*qy));
00264     //double pitch = factor * asin((double)(2 * (qw*qy - qx*qz)));      //reversed
00265     double pitch_conversion = factor * asin((double)(2 * (qx*qz - qw*qy)));
00266     double heading_conversion = factor * atan2((double)(qw*qz + qx*qy), (double)(0.5 - qy*qy - qz*qz));
00267     heading_conversion=(-1*heading_conversion);
00268     if (heading_conversion<0) {
00269         heading_conversion+=360;   //quaternion reads heading in CCW, flip to CW
00270     }
00271     //pc.printf("log: Q-derived h: %lf, r: %lf, P: %lf\n\r", heading_conversion, roll_conversion, pitch_conversion);
00272     conv->r = int(roll_conversion*16);
00273     conv->h = int(heading_conversion*16);
00274     conv->p = int(pitch_conversion*16);
00275 }
00276 
00277 //Data function pulls data from BNO and sends over serial
00278 //Timed using DigitalOut and Oscope. 
00279 //  At baud 115200, averaged 5 times over 256, +pulsewidth 11.1 - 13.3ms.
00280 //  At baud 921600, averaged over 256, +pw 4.1 - 5.5ms
00281 //  In water, 921600 induced serial comm errors
00282 //  Variance is due to MS5837 pressure sensor. Includes waits of 2-4ms.
00283 void az_data() {
00284     //function_timer2=1;
00285 
00286     leak_data();
00287     uint32_t k=0x1234abcd;
00288 
00289     // if (logic_available==1) ready_data=(ready_data&0xfbff);
00290     // else ready_data=(ready_data|0x0400);
00291     if (call_threads_available==1) ready_data=(ready_data & 0xfdff);
00292     else ready_data=(ready_data | 0x0200);
00293     // if (command_available==1) ready_data=(ready_data&0xfeff);
00294     // else ready_data=(ready_data | 0x0100);
00295     if (zero_set==1) ready_data=(ready_data|0x0008);
00296     else ready_data=(ready_data&0xfff7);
00297 
00298     //Instantiate status array of 7 32-bit words.
00299     //First 16 bits of each 32-bit word are Identifiers for RasPi to correctly assign the trailing 16 bits of data.
00300     uint32_t status[14]={0};
00301     uint32_t ready=ready_prefix;
00302     ready=(ready<<16)|ready_data;
00303 
00304     //word 0: Key
00305         //Used to ensure Pi and Mbed are on same page.
00306     status[0]=k;
00307 
00308     //word 1: Status information.
00309         //0xffff acts as prefix to identify Status for RasPi.
00310         //Last 3 bits (from right) are current position (POS[0-7]). See BNO datasheet.
00311         //4th bit (from right) is RH turn motors enabled.
00312         //5th bit (from right) is LH turn motors enabled.
00313     status[1]=ready;
00314 
00315     //word 2: Calibration.
00316         //0xc000 acts as prefix to identify Cal for RasPi.
00317     status[2]=0xc0000000+imu.read_calib_status();
00318 
00319     //Get Euler data from BNO.
00320 //    imu.get_Euler_Angles(&euler_angles);
00321     imu.get_quaternion(&quaternion);
00322     double qw=(double)quaternion.w;
00323     double qx=(double)quaternion.x;
00324     double qy=(double)quaternion.y;
00325     double qz=(double)quaternion.z;
00326     q_to_E(qw,qx,qy,qz,&conversion);
00327     
00328     //word 3 is Heading.
00329         //0xc100 acts as prefix to identify Heading for RasPi. 
00330     //uint16_t h = euler_angles.h;
00331     uint16_t h = conversion.h;
00332     heading=h;
00333     status[3]=0xc1000000+h;
00334 
00335     //Offset calculation: 360*16bit resolution = 5760 -> converted to hex = 0x1680
00336     int offset=0x1680;
00337 
00338     //word 4 is Roll.
00339         //0xc300 acts as prefix to identify Roll for RasPi.
00340         //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
00341     //uint16_t r = offset + euler_angles.r;
00342     uint16_t r = offset + conversion.r;
00343     status[4]=0xc3000000+r;
00344 
00345     //word 5 is Pitch.
00346         //0xc500 acts as prefix to identify Pitch for RasPi.
00347         //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
00348     //uint16_t p = offset + euler_angles.p;
00349     uint16_t p = offset + conversion.p;
00350     pitch=(p - (offset/2)); //only want 180deg offset
00351     status[5]=0xc5000000+p;
00352     
00353     //word 6 gets Depth from el_data() function.
00354         //0xb0 acts as prefix to identify Barometer Pressure.
00355     // status[6]=el_data();
00356     
00357     //Thruster speeds in 10us resolution.
00358     status[6]=((fwd_port.thruster_data() & 0x00ffffff) | 0xf1000000);
00359     status[7]=((fwd_star.thruster_data() &0x00ffffff) | 0xf2000000);
00360     status[8]=((aft_port.thruster_data() & 0x00ffffff) | 0xf3000000);
00361     status[9]=((aft_star.thruster_data() & 0x00ffffff) | 0xf4000000);
00362 
00363     //Gains in format: ABC * 10^(D-9), similar to resistor color bands but starting at 10^-9 rather than 10^0.
00364     //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]
00365     
00366     //Heading Gains
00367     status[10]= 0xd1100000 + k_data(heading_Kp); //Kp
00368     status[11]= 0xd1200000 + k_data(heading_Ki); //Ki
00369     status[12]= 0xd1300000 + k_data(heading_Kd); //Kd
00370 
00371     //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi.
00372     int i;
00373     int l=(sizeof(status)/sizeof(uint32_t))-1;
00374     for (i=0; i<=l; i++) {
00375         pc.printf("%x\n", status[i]);
00376   }
00377   //function_timer2=0;
00378 }
00379 
00380 //reset all controller set points and zero out any accumulated integral gains to ESCs
00381 void stop_all_persistent() {
00382     persistent_heading=-1;
00383     persistent_speed=-1;
00384     pid_heading.clear();
00385     // pid_speed.reset();
00386 }
00387 
00388 //When bad things are happening.
00389 void EventHorizon() {
00390     event_horizon_flag=1;
00391     stop_all_persistent();
00392     horizon_count++;
00393     pc.printf("log: EventHorizon called, count: %i\r\n",horizon_count);
00394     //setEvent() method locks out Thruster class set_speed() function
00395     //  and sets PWM to 1.5ms.
00396     fwd_port.setEvent();
00397     fwd_star.setEvent();
00398     aft_port.setEvent();
00399     aft_star.setEvent();
00400     pc.printf("log: Thruster events successfully set\r\n");
00401     //Tells Raspi that Emergency state has been initiated.
00402     ready_prefix=(horizon_prefix+horizon_count);
00403     //Wait some time during which Thruster set_speed() functions are locked out.
00404     for (int i=0; i<200; i++) {
00405         //Resume streaming data to RasPi during timeout period.
00406         az_data();
00407         wait_ms(wait_main);
00408     }
00409     //Clear emergency situation.
00410     fwd_port.clearEvent();
00411     fwd_star.clearEvent();
00412     aft_port.clearEvent();
00413     aft_star.clearEvent();
00414     pc.printf("log: Thruster events successfully cleared\r\n");
00415     //Set PWM to 1.5ms after emergency situation. Should have been set to 1.5ms, but double checking.
00416     //  For extra precaution.
00417     stop_all_persistent();
00418     //Lets Raspi know that Mbed is ready for commands again.
00419     ready_prefix=0xffff;
00420     pc.printf("log: ready status reset, mbed may resume\r\n");
00421     event_horizon_flag=0;
00422 }
00423 
00424 //Commands function handles Serial input, checks for correct syntax, and calls appropriate function(s) to execute commands.
00425 int read_serial() {
00426     int i=0;
00427     while (pc.readable()) {
00428         readline[i]=pc.getc();
00429         pc.printf("log: input read %c at %i\r\n",readline[i],i);
00430         i++;
00431     }
00432     //pc.printf("i: %i\r\n",i);
00433     return i;
00434 }
00435 int look_horizon() {
00436     int returnval=0;
00437     pc.printf("log: THREAD START, horizon\r\n");
00438     //int length=0;
00439         //if (length==4) {
00440     char check_HORIZON[5]="STOP";
00441     int verified_HORIZON=0;
00442     for (int i=0; i<5; i++) {
00443         if (readline[i]==check_HORIZON[i]) verified_HORIZON++;
00444     }
00445     if (verified_HORIZON==4) {
00446         EventHorizon();
00447         returnval=1;
00448     }
00449         //}
00450     pc.printf("log: THREAD END, horizon\r\n");
00451     return returnval;
00452 }
00453 
00454 //TODO: for lateral movement
00455 //double offsetController() {
00456 //}
00457 
00458 //Function to go to set heading.
00459 //Timed with DigitalOut on Oscope.
00460 //With no heading set, 28.6us.
00461 //With heading calculations, ~32.8us
00462 //With logging added, ~1.278ms  -> serial print statements induce significant delays
00463 double headingController() {
00464     // call_threads_available=0;
00465     double diff=0;
00466     double speed=0;
00467     double dir=1;
00468     double desired_heading=persistent_heading;
00469     double current_heading=heading;
00470     current_heading=(current_heading/16);
00471     if (desired_heading!=-1) {
00472         //Calculate how far to turn in degrees.
00473         diff = abs(desired_heading-current_heading);
00474         //pc.printf("log: diff before if: %f\r\n",diff);
00475 
00476         //Correct for 360-0 edge cases if 'diff'erence is greater than 180.
00477         //Change direction and recalculate for accurate 'tolerance' comparison.
00478         if (diff>180.0) {
00479             //pc.printf("log: diff>180: %f\r\n",diff);
00480             if (desired_heading>180) {
00481                 current_heading=current_heading+180;
00482                 desired_heading=desired_heading-180;
00483                 pc.printf("log: desired>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff);
00484             }
00485             else {  //current_heading>180
00486                 current_heading=current_heading-180;
00487                 desired_heading=desired_heading+180;
00488                 pc.printf("log: current>180, desired: %f, current: %f, dir: %f, diff: %f\r\n",desired_heading,current_heading,dir,diff);
00489             }
00490         }
00491         speed = pid_heading.process(desired_heading,current_heading);
00492         // pc.printf("log: des %f, cur %f, pr speed %f\r\n",desired_heading,current_heading,speed);
00493 
00494         //Necessary to overcome 25us deadzone around 1.5ms
00495         if ((abs(speed)<min_thruster_bias) and (abs(diff)>az_tolerance)) {
00496             if (speed<0) speed=(-1*min_thruster_bias);
00497             else speed=min_thruster_bias;
00498         }
00499     }
00500     // call_threads_available=1;
00501     return (speed*dir);
00502 }
00503 
00504 //Controller to maintain the scalar component of velocity vector for starboard front and port aft thrusters.
00505 double cos_speedController() {
00506     // call_threads_available=0;
00507     double offset_factor=0;
00508     if (persistent_offset!=-1) {
00509         offset_factor=(((2*PI_math)/360)*persistent_offset);
00510     }
00511     offset_factor+=(PI_math/4);
00512     double desired_speed=persistent_speed*cos(offset_factor);
00513     desired_speed=(desired_speed/1000);     //convert int us to ms
00514 
00515     // call_threads_available=1;
00516     return (desired_speed);
00517 }
00518 
00519 //Controller to maintain the scalar component of velocity vector for port front and starboard aft thrusters.
00520 double sin_speedController() {
00521     // call_threads_available=0;
00522     double offset_factor=0;
00523     if (persistent_offset!=-1) {
00524         offset_factor=(((2*PI_math)/360)*persistent_offset);
00525     }
00526     offset_factor+=(PI_math/4);
00527     double desired_speed=persistent_speed*sin(offset_factor);
00528     desired_speed=(desired_speed/1000);     //convert int us to ms
00529 
00530     // call_threads_available=1;
00531     return (desired_speed);
00532 }
00533 
00534 // Make superposition of all Controllers accessing thrusters acting on Az plane.
00535 //  Only function that shall write to Az plane thrusters.
00536 //  This will also deprecate call_threads() function.
00537 void az_thruster_logic() {
00538     if (manual_mode==0) {
00539 //        function_timer3=1;
00540         // logic_available=0;
00541         double heading_speed=0;
00542         double sin_speed=0;
00543         double cos_speed=0;
00544         double fwd_star_speed=0;
00545         double fwd_port_speed=0;
00546         double aft_star_speed=0;
00547         double aft_port_speed=0;
00548         if (persistent_speed==-1) {
00549             cos_speed=0;
00550             sin_speed=0;
00551         }
00552         if (persistent_heading==-1) heading_speed=0;
00553         if (persistent_heading!=-1) {
00554             heading_speed=headingController();
00555         }
00556         if (persistent_speed!=-1) {
00557             cos_speed=cos_speedController();
00558             sin_speed=sin_speedController();
00559         }
00560 
00561         //Create Superposition of Heading and Speed
00562         fwd_star_speed=(cos_speed - heading_speed);
00563         aft_port_speed=(cos_speed + heading_speed);
00564         fwd_port_speed=(sin_speed + heading_speed);
00565         aft_star_speed=(sin_speed - heading_speed);
00566 
00567         //Error checking to ensure PWM doesn't escape ESC parameters
00568         if (fwd_port_speed<(-1*esc_range)) fwd_port_speed=(-1*esc_range);
00569         if (fwd_star_speed<(-1*esc_range)) fwd_star_speed=(-1*esc_range);
00570         if (aft_star_speed<(-1*esc_range)) aft_star_speed=(-1*esc_range);
00571         if (aft_port_speed<(-1*esc_range)) aft_port_speed=(-1*esc_range);
00572 
00573         if (fwd_port_speed>esc_range) fwd_port_speed=esc_range;
00574         if (fwd_star_speed>esc_range) fwd_star_speed=esc_range;
00575         if (aft_star_speed>esc_range) aft_star_speed=esc_range;
00576         if (aft_port_speed>esc_range) aft_port_speed=esc_range;
00577 
00578         //Write PWM values.
00579         fwd_port.set_speed(fwd_port_speed);
00580         aft_port.set_speed(aft_port_speed);
00581         //pc.printf("log: port %f\r\n",port_speed);
00582         fwd_star.set_speed(fwd_star_speed);
00583         aft_star.set_speed(aft_star_speed);
00584         //pc.printf("log: stbd %f\r\n",starboard_speed);
00585         //logic_available=1;
00586         //function_timer3=0;
00587     }
00588 }
00589 
00590 
00591 //Function to change BNO position.
00592 void switch_pos(int position) {
00593     uint16_t ready_mask=0xfff8;
00594     if (position>=0 and position<8) {
00595         switch (position) {
00596             case 1:
00597                 imu.set_mounting_position(MT_P1);
00598                 ready_data=((ready_data & ready_mask)+0x001);
00599                 break;
00600             case 2:
00601                 imu.set_mounting_position(MT_P2);
00602                 ready_data=((ready_data & ready_mask)+0x002);
00603                 break;
00604             case 3:
00605                 imu.set_mounting_position(MT_P3);
00606                 ready_data=((ready_data & ready_mask)+0x003);
00607                 break;
00608             case 4:
00609                 imu.set_mounting_position(MT_P4);
00610                 ready_data=((ready_data & ready_mask)+0x004);
00611                 break;
00612             case 5:
00613                 imu.set_mounting_position(MT_P5);
00614                 ready_data=((ready_data & ready_mask)+0x005);
00615                 break;
00616             case 6:
00617                 imu.set_mounting_position(MT_P6);
00618                 ready_data=((ready_data & ready_mask)+0x006);
00619                 break;
00620             case 7:
00621                 imu.set_mounting_position(MT_P7);
00622                 ready_data=((ready_data & ready_mask)+0x007);
00623                 break;
00624             case 0:
00625             default:
00626                 imu.set_mounting_position(MT_P0);
00627                 ready_data=((ready_data & ready_mask));
00628                 break;
00629         }
00630     }
00631 }
00632 
00633 //Function to change BNO Fusion Modes.
00634 void switch_mode(uint8_t new_mode) {
00635     imu.change_fusion_mode(new_mode);
00636     pc.printf("log: change BNO mod to %u\r\n",new_mode);
00637     // int switch_int = (int)new_mode;
00638     // switch (switch_int) {
00639         // case 1:
00640             // imu.change_fusion_mode(MODE_M4G);
00641             // pc.printf("log: change BNO mode to M4G\r\n");
00642             // break;
00643         // case 2:
00644             // imu.change_fusion_mode(MODE_COMPASS);
00645             // pc.printf("log: change BNO mode to COMPASS\r\n");
00646             // break;
00647         // case 3:
00648             // imu.change_fusion_mode(MODE_IMU);
00649             // pc.printf("log: change BNO mode to IMU\r\n");
00650             // break;
00651         // case 12:
00652         // default:
00653             // imu.change_fusion_mode(MODE_NDOF);
00654             // pc.printf("log: change BNO mode to NDOF\r\n");
00655             // break;
00656     // }
00657 }
00658 
00659 //Function for manual up/down, left/right controls.
00660 //Takes current measurement of relevant sensor and adds/subtracts the "resolution" times "magnitude" 
00661 //      to new set point, incrementally driving vehicle.
00662 //      ie current heading 10deg, function(3,-1), new heading set point of 10+(-1*3) = 7deg
00663 void increment_persistent(int select, int magnitude) {
00664     int heading_resolution=3;   //degrees
00665     int speed_resolution=27;     //us
00666     switch (select) {
00667         //heading
00668         case 3:
00669             persistent_heading=((heading/16)+(magnitude*heading_resolution));
00670             pid_heading.clear();
00671             break;
00672         
00673         //speed
00674         case 4:
00675             persistent_speed=((1000*fwd_star.get_speed())+(magnitude*speed_resolution));
00676             // pid_speed.clear();
00677             break;
00678         
00679         //offset
00680         case 5:
00681             persistent_offset=(persistent_offset+(magnitude*heading_resolution));
00682             break;
00683     }
00684 }
00685 
00686 float k_commands(int value, int power) {
00687     static float pow[10] = {
00688         0.00001, 0.0001, 0.001, 0.01, 0.1,  //[0,4]
00689         1, 10, 100, 1000, 10000 //[5,9]
00690     };
00691     float return_val=0;
00692     return_val = value*pow[power];
00693     pc.printf("log: return %f, pow %lf\r\n",return_val,pow[power]);
00694     return return_val;
00695 }
00696 
00697 //Function intakes serial commands.
00698 void commands() {
00699     //pc.printf("log: commands called\r\n");
00700     int length=0;
00701     length=read_serial();
00702     if (length==4) {
00703         look_horizon();
00704     }
00705     if (length==7) {
00706         char input[10];
00707         for (int i=0; i<10; i++) {
00708             input[i]=readline[i];
00709             //pc.printf("Command thread: read %c at %i\r\n",readline[i],i);
00710         }
00711         //check_pos char array used to filter Position commands.
00712         char check_pos[5]="pos:";
00713         //check_mod char array used to filter Fusion modes on BNO.
00714         char check_mod[5]="mod:";
00715         //Heading commands.
00716         char check_hea[5]="hea:";
00717         //Speed commands.
00718         char check_vel[5]="vel:";
00719         //Offset commands.
00720         char check_off[5]="off:";
00721         //Stop all commands.
00722         char check_sto[5]="sto:";
00723         //Reset Mbed command.
00724         char check_res[5]="res:";
00725 
00726         //Heading PID gains
00727         char check_hkp[5]="hkp:";
00728         char check_hki[5]="hki:";
00729         char check_hkd[5]="hkd:";
00730         
00731         //While loop reads Serial input into input buffer.
00732 
00733         //Only continue if input buffer has 7 entries, otherwise ignore input buffer. 
00734         //All commands from RasPi shall come in a format of 7 characters "abc:xyz" 
00735         //      where 'abc' is an identifying string and 'xyz' is some data/information.
00736     //    if (i==7) {
00737             //Instantiate counters that will be used to verify known commands.
00738         int verified_pos=0;
00739         int verified_mod=0;
00740         int verified_hea=0;
00741         int verified_sto=0;
00742         int verified_res=0;
00743         int verified_vel=0;
00744         int verified_off=0;
00745 
00746         int verified_hkp=0;
00747         int verified_hki=0;
00748         int verified_hkd=0;
00749         
00750         //While loop checks first 4 characters of input buffer and attempts to match
00751         //      against known commands.
00752         for (int q=0; q<3; q++) {
00753             //Increment verified_pos if a match is found between Serial input buffer
00754             //      and Position command format.
00755             if (input[q]==check_pos[q]) {
00756                 verified_pos++;
00757                 //pc.printf("pos %c at %i\r\n",input[q],q);
00758             }
00759             //Increment verified_mod if a match is found between Serial input buffer
00760             //      and Fusion Modde command format.
00761             if (input[q]==check_mod[q]) {
00762                 verified_mod++;
00763                 //pc.printf("mod %c at %i\r\n",inputtt[q],q);
00764             }
00765             //Increment verified_hea if a match is found between Serial input buffer
00766             //      and Heading command format.
00767             if (input[q]==check_hea[q]) {
00768                 //pc.printf("hea %c at %i\r\n",input[q],q);
00769                 verified_hea++;
00770             }
00771             if (input[q]==check_sto[q]) {
00772                 //pc.printf("sto %c at %i\r\n",input[q],q);
00773                 verified_sto++;
00774             }
00775             if (input[q]==check_res[q]) {
00776                 //pc.printf("res %c at %i\r\n",input[q],q);
00777                 verified_res++;
00778             }
00779             if (input[q]==check_vel[q]) {
00780                 verified_vel++;
00781                 //pc.printf("vel %c at %i\r\n",input[q],q);
00782             }
00783             if (input[q]==check_off[q]) {
00784                 verified_off++;
00785             }
00786             if (input[q]==check_hkp[q]) {
00787                 verified_hkp++;
00788             }
00789             if (input[q]==check_hki[q]) {
00790                 verified_hki++;
00791             }
00792             if (input[q]==check_hkd[q]) {
00793                 verified_hkd++;
00794             }
00795         }
00796 
00797         //If first 4 characters from Serial input buffer match Position command format,
00798         //      execute "switch_pos" function.
00799         if (verified_pos==3) {
00800             int change=(input[6]-'0');
00801             switch_pos(change);
00802         }
00803         if (verified_mod==3) {
00804             uint8_t mode=(input[6]-'0');
00805             if ((mode>0x0) and (mode<0x0d)) {
00806                 switch_mode(mode);
00807             }
00808         }
00809         if (verified_sto==3) {
00810             //pc.printf("log: stop command received\r\n");
00811             stop_all_persistent();
00812             //pc.printf("log: stop command executed\r\n");
00813         }
00814         //If first 4 characters from Serial input buffer match Heading command format,
00815         //      execute "motors" function.
00816         if (verified_hea==3) {
00817             //Correct for ascii '0', and reform 3digit decimal number
00818             int hea100=(input[4]-'0');
00819             int hea10=(input[5]-'0');
00820             int hea1=(input[6]-'0');
00821             int hea=(hea100*100)+(hea10*10)+(hea1*1);
00822             pc.printf("log: heading rx: %i\r\n",hea);
00823             if (hea==999) {
00824                 persistent_heading=-1;
00825                 pid_heading.clear();
00826             }
00827             if ((hea>=831) and (hea<=837)) {
00828                 increment_persistent(hea10,(hea1-4));
00829             }
00830             if ((hea>=0) and (hea<=360)) {
00831                 pid_heading.clear();
00832                 if (event_horizon_flag==0) persistent_heading=hea;
00833             }
00834         }   
00835         if (verified_res==3) {
00836             pc.printf("log: Reset mbed received. See you on the other side.\r\n");
00837             //Mbed reset command.
00838             NVIC_SystemReset();
00839             //If this print statement is ever executed, reset didn't happen.
00840             pc.printf("log: Reset failed. The show goes on.\r\n");
00841         }
00842         // Forward/Reverse speed commands.
00843         if (verified_vel==3) {
00844             int vel100=(input[4]-'0');
00845             int vel10=(input[5]-'0');
00846             int vel1=(input[6]-'0');
00847             int vel=(vel100*100)+(vel10*10)+(vel1*1);
00848             pc.printf("log: vel rx: %i\r\n",vel);
00849             //999 to stop speed controller.
00850             if (vel==999) {
00851                 persistent_speed=-1;
00852             }
00853             if ((vel>=841) and (vel<=847)) {
00854                 increment_persistent(vel10,(vel1-4));
00855             }
00856             if ((vel>=0) and (vel<=800)) {
00857                 // pid_speed.clear();
00858                 vel=(vel-400);
00859                 if (event_horizon_flag==0) persistent_speed=vel;
00860             }
00861         }
00862         
00863         //Heading offset commands.
00864         if (verified_off==3) {
00865             int off100=(input[4]-'0');
00866             int off10=(input[5]-'0');
00867             int off1=(input[6]-'0');
00868             int off=(off100*100)+(off10*10)+(off1*1);
00869             pc.printf("log: off rx: %i\r\n",off);
00870             //999 to reset heading offset to 0.
00871             if (off==999) {
00872                 persistent_offset=-1;
00873             }
00874             if ((off>=851) and (off<=857)) {
00875                 increment_persistent(off10,(off1-4));
00876             }
00877             if ((off>=0) and (off<=360)) {
00878                 if (event_horizon_flag==0) persistent_offset=off;
00879             }
00880         }
00881 
00882         int reset_h=0;
00883         if (verified_hkp==3) {
00884             float kval=((input[4]-'0')*10)+input[5]-'0';
00885             heading_Kp=k_commands(kval,input[6]-'0');
00886             reset_h=1;
00887         }
00888         if (verified_hki==3) {
00889             float kval=((input[4]-'0')*10)+input[5]-'0';
00890             heading_Ki=k_commands(kval,input[6]-'0');
00891             reset_h=1;
00892         }
00893         if (verified_hkd==3) {
00894             float kval=((input[4]-'0')*10)+input[5]-'0';
00895             heading_Kd=k_commands(kval,input[6]-'0');
00896             reset_h=1;
00897         }
00898         if (reset_h) {
00899             pid_heading.set_K(heading_Kp,heading_Ki,heading_Kd);
00900             pc.printf("log: update Heading gains Kp %f, Ki %f, Kd %f\r\n",heading_Kp, heading_Ki, heading_Kd);
00901         }
00902     }
00903 }
00904 
00905 //Initialize controllers associated with Azimuth, speed and heading.
00906 void init_AzController(void){
00907     // Zero out PID values.
00908     // pid_speed.clear();
00909     pid_heading.clear();
00910     
00911     // run Az controller as set in globals, in ms
00912     tickerAzThrusters.attach(&az_thruster_logic, ticker_rate);   
00913 }
00914 
00915 int main() {
00916     //engage plaidspeed
00917     pc.baud(baudrate);
00918 
00919     //Uncomment to derive timing using Oscope.
00920     //function_timer=0;
00921     //function_timer2=0;
00922     //function_timer3=0;
00923 
00924     //If not ready, reset BNO.
00925     while (imu.chip_ready() == 0) {
00926         do {
00927             pc.printf("resetting BNO\r\n");
00928             pwr_on=0;
00929             wait_ms(100);
00930             pwr_on=1;
00931             wait_ms(50);
00932         } while(imu.reset());
00933     }
00934     wait_ms(20);
00935 
00936     //If BNO is ready, set ready status indication
00937     if (imu.chip_ready()) {
00938         ready_prefix=0xffff;
00939     }
00940     switch_pos(0);  //BNO default position 1
00941     switch_mode(0x08);      //0x08      MODE_IMU
00942 
00943     init_AzController();
00944 
00945     //Look for serial input commands and send to 'commands' function.
00946     //If no serial input commands, stream data.
00947     while(1) {
00948         if (pc.readable()) {
00949             // command_available=0;
00950             commands();
00951             //function_timer4=1;
00952             // command_available=1;
00953         }
00954         else {
00955             az_data();
00956         }
00957         wait_ms(wait_main/2);
00958         //function_timer4=0;
00959         wait_ms(wait_main/2);
00960     }
00961 }
00962