/* * File: QuadMain.c * Author: John J. Walsh * * Created on April 3, 2012, 4:31 PM */ /****************************************************************************** * Software License Agreement * * Copyright © 2012 RadioFlyers and Walsh Inc. All rights reserved. * RadioFlyers licenses to you the right to use, modify, copy, distribute, own * play with, tinker around, test, modify again, get pissed at, and insult * this code only when done at the University of Notre Dame, or in fact whenever * you feel like it. It is recommended, however, that should you be just * starting this project now, that you immediately stop and pick another * project. We would recommend a project that cooks steak. This way you get * to eat steak with the University's money. That or a perpetual motion * machine. Those are pretty baller and probably much simplier than this * project is. If, however, you have finalized the project and are stuck with * it, I recommend the following steps. Move the computer to the side, stand * up, lean, bend over, pray to whichever god you like, kiss your ass goodbye. * * You should refer to the individual comments for each section and function * for specific concerns and questions. Just about all of this code and * comments were written by John J. Walsh (except anything in the PWM.h file. * all of that was writen by Jay Burns, who coincidentaly is a pilot....who * knew....). I take great pride in the fact that I didn't comment shit while * writing the code, and have gone back now and put in these comments for your * damn benifit. That being said, this is being done during finals week, * second semester senior year, so my fucks-given meter is at a pretty all time * low. So, if comments are not to your liking, and you don't understand what's * going on, feel free to get a piece of paper, write down your question/comment, * walk to the nearest trash can, and throw the piece of paper on the floor as * hard as you can... On a serious note, if you manage to read all this, and * figure out a way to contact me, I would be thuroughly impressed and would * probably answer any question you had. * *****************************************************************************/ #if defined(__PIC24E__) #include #elif defined (__PIC24F__) #include #elif defined(__PIC24H__) #include #elif defined(__dsPIC30F__) #include #elif defined (__dsPIC33E__) #include #elif defined(__dsPIC33F__) #include #endif #define FCY 16000000UL // 16 MHz cycle time for delay routines #include #include"TerminalUart.h" #include #include #include //#include "Analog.h" #include #include"I2C.h" #include"Misc.h" #include"PWM.h" #include //_CONFIG3(SOSCSEL_EC) _CONFIG1 (FWDTEN_OFF & JTAGEN_OFF ) _CONFIG2 (PLLDIV_NODIV & FNOSC_FRCPLL & FCKSM_CSECME & POSCMOD_NONE) int MotorThrusts[4]; // Function Prototypes int main(void); int main(void) { /************************************** * Initializing variables **************************************/ MotorThrusts[0] = 0; MotorThrusts[1] = 0; MotorThrusts[2] = 0; MotorThrusts[3] = 0; char string[20]; int NewTotalThrust = 0, TotalThrust = 0, DeltaThrust = 0; TotalThrust = 0; int i; bool first = 1; bool Switch = 1; float targetX = 0; float targetY = 0; unsigned int pulses; float DerivativeX = 0, DerivativeY = 0, IntegralX = 0, IntegralY = 0, errorX = 0, errorY = 0, lastErrorX = 0, lastErrorY = 0, NextIntX = 0, NextIntY = 0, time = 0, temp, ThetaX, ThetaY; short int Px = 0, Py = 0, Ix = 0, Iy = 0, Dx = 0, Dy = 0, Change23 = 0, Change34 = 0; int MotMin = 0, TempDel1 = 0, TempDel2 = 0, Delta = 0, MotMax = 0; /***************************************************************************** * These are the values that you change to manipulate the P.I.D controler * The max values are the maximum contribution each of the components can have * Kp is for the proportional term * Ki is for the integral term * Kd is for the derivative term *****************************************************************************/ int Imax = 4, Pmax = 4, Dmax = 3; float Kp = 0.6; float Ki = 1; float Kd = 0.1; /*************************************************************************** * Here starts the initialization routine ***************************************************************************/ init_usart(103); myputc('\f'); myputs("Initializing."); init_I2C(0x35); myputc('.'); Acc_init(); myputc('.'); Mag_init(); myputc('.'); Bar_getParam(); myputc('.'); Gyro_init(); myputc('.'); initializePWMoutput(); myputc('.'); initializePWMcapture(); myputc('.'); init_TMR1(); myputc('.'); /*************************************************************************** * This is just a simple stoping mechanism to stop the code before all the * offsets are determined. This allows one to ensure the QuadRotor is level ***************************************************************************/ myputc('\n'); myputc('\r'); myputs("Ensure switch is off..."); getPWM(); while(Switch == 1) { readCH5(); Switch = CH5status; } myputc('\n'); myputc('\r'); myputs("Ensure QuadRotor is level and stable"); myputc('\n'); myputc('\r'); myputs("Getting offsets."); Acc_getZoff(); myputc('.'); Gyro_getOff(); PrintOffs(); /*************************************************************************** * This is put in here to initialize the ESCs should they need it. Otherwise * one can take the time here to test out the throtle. If the ESCs need to be * calibrated put the throtle in low, then high for 4 sec, then low again. ***************************************************************************/ myputc('\n'); myputc('\r'); myputs("Turn switch on to start initialization..."); while(Switch == 0) { readCH5(); Switch = CH5status; } myputc('\n'); myputc('\r'); myputs("Turn switch off when finished..."); while(Switch == 1) { getPWM(); Switch = CH5status; setPower(CH3value, CH3value, CH3value, CH3value); } myputc('\n'); myputc('\r'); myputs("Turn switch on when ready to fly!"); while(Switch == 0) { readCH5(); Switch = CH5status; } /*************************************************************************** * Here begins the main loop of the code. The outer while loop is to run * constantly and to allow us to have a kill switch (currently on ch 5) to * turn the QuadRotor off and on at any point ***************************************************************************/ while (1){ readCH5(); CH5status = 1; Switch = CH5status; for (i = 0; i<4; i++) { MotorThrusts[i] = 0; // set all motors to 0 } TotalThrust = 0; setPower(0, 0, 0, 0); IntegralX = 0; // clear integral terms of PID IntegralY = 0; first = 1; // clear derivative terms of PID lastErrorX = 0; lastErrorY = 0; /*************************************************************************** * This starts the main loop inside the main loop. This is primarily to * run our PID controler ***************************************************************************/ while (Switch == 1) { ///////////////////////// // Get Thrust input ///////////////////////// getPWM(); Switch = CH5status; CH3value = 25; NewTotalThrust = CH3value*4; // Turn input into 'overall thrust input' MotMax = CH3value + 20; if (MotMax > 170) { MotMax = 170; } MotMin = CH3value - 10; if (MotMin < 0) { MotMin = 0; } DeltaThrust = (NewTotalThrust - TotalThrust)>>2; // Change for each motor (initially all increased equally) TotalThrust = NewTotalThrust; for (i = 0; i<4; i++) { MotorThrusts[i] = MotorThrusts[i]+DeltaThrust; // Increase all the motors equally } ///////////////////////// // Get data ///////////////////////// Acc_getAccVec(); Mag_getMagVec(); Gyro_getRotVec(); ///////////////////////// // Print Data ///////////////////////// //PrintData(); /* * Quick attempt at determining Theta values. Not entirely sure whether * it really works or not, gave up on it due to time ThetaX = atan2(AccVec[1],AccVec[2]); ThetaY = atan2(AccVec[0],AccVec[2]); myputc('\n'); myputc('\r'); sprintf(string, "%f",ThetaX); myputs(string); myputc(','); sprintf(string, "%f",ThetaY); myputs(string); myputc('\n'); myputc('\r'); */ //////////////////////////////////////// // QuadRotor Orientation & motor numbers + for counter-clockwise - for clockwise // (-)2 1(+) // \ / |y // |^| |__ x //Motor locations and axis // / \ // (+)3 4(-) //////////////////////////////////////// /////////////////////////////////// // P.I.D. Controller // 3 sets to change, motors 1&4, motors 1&2, and motors 1&3 // correspond to X accel, Y accel, and Z rot ////////////////////////////////// /*************************************************************************** * P section of PID controler. This controls proportional response to * some offset. Our PID controler is simply trying to drive acceleration * in the X and Y directions relative to the QuadRotor frame to zero. This * sort of works, but I would advise you to try and develop system to * determine orientation angles and drive those to given value ***************************************************************************/ errorX = AccVec[0]-targetX; errorY = AccVec[1]-targetY; //errorZ = RotVec[3]-targetZ; // Never got to implementing it for rotations about z axis if (errorX >=0) { Px = ceil(Kp * round(errorX*10)); // Nets a range over which Px is increased } // 0->0.05 = 0, 0.05->0.3 = 1, 0.3->0.6 = 2, 0.6-> = 3 else { Px = floor(Kp * round(errorX*10)); } if (errorY >=0) { Py = ceil(Kp * round(errorY*10)); // Nets a range over which Px is increased } // 0->0.05 = 0, 0.05->0.3 = 1, 0.3->0.6 = 2, 0.6-> = 3 else { Py = ceil(Kp * round(errorY*10)); } /* myputc('<'); myputi(Px); myputc(','); myputi(Py); myputc('>'); */ /*************************************************************************** * This starts the integral part of the PID. First thing to do is determine * the amount of time that has passed. Pretty sure the algorithm to do * that works, but never fully tested it. One might want to look into it. ***************************************************************************/ if (first) // In here to disregard data from first loop through { first = !first; time = 0; } else { pulses = TMR1; TMR1 = 0; temp = (float)pulses; time = temp*64/16000000; } if ( errorX>=-0.05 && errorX<=0.05) { NextIntX = 0; } ///////////////////////////////////////////// else // yields increase of 1 if error is 0.1 for 1 sec { NextIntX = errorX*10*time; } if ( errorY>=-0.05 && errorY<=0.05) { NextIntY = 0; } else { NextIntY = errorY*10*time; } IntegralX = IntegralX + NextIntX; IntegralY = IntegralY + NextIntY; if (IntegralX > Imax) { IntegralX = Imax; } else if (IntegralX < Imax*-1) { IntegralX = -1*Imax; } if (IntegralY > Imax) { IntegralY = Imax; } else if (IntegralY < Imax*-1) { IntegralY = -1*Imax; } //IntegralZ = IntegralZ + errorZ*time; if (errorX >=0) { Ix = floor(Ki*IntegralX); } else { Ix = ceil(Ki*IntegralX); } if (errorY >=0) { Iy = floor(Ki*IntegralY); } else { Iy = ceil(Ki*IntegralY); } /* myputc('<'); myputi(Ix); myputc(','); myputi(Iy); myputc('>'); */ /*************************************************************************** * This begins the derivative section of the PID controler. Never really * had a good grasp of what I wanted the derivative term to do, so kinda * just pulled numbers out of thin air to come up with oridinal equation. * Might want to come up with better way of determining Derivative terms. * Also, hesitant about its full functionality because Derivative first * time throught should be infinity (because time is 0), but it doesn't * seem to be. Should check that out. ***************************************************************************/ DerivativeX = (errorX-lastErrorX)/time; DerivativeY = (errorY-lastErrorY)/time; //DerivativeZ = (errorZ-lastErrorZ)/time; if (errorX >=0) { Dx = floor(DerivativeX*Kd); //////////////////// } // Yields change of 1 for change of 0.1g in 0.1 sec else { Dx = ceil(DerivativeX*Kd); } if (errorY >=0) { Dy = floor(DerivativeY*Kd); //////////////////// } // Yields change of 1 for change of 0.1g in 0.1 sec else { Dy = ceil(DerivativeY*Kd); } /* myputc('<'); myputi(Dx); myputc(','); myputi(Dy); myputc('>'); */ /*************************************************************************** * Ensure all values under max ***************************************************************************/ if (Px > Pmax) { Px = Pmax; } else if (Px < -1 * Pmax) { Px = -1 * Pmax; } if (Py > Pmax) { Py = Pmax; } else if (Py < -1 * Pmax) { Py = -1 * Pmax; } if (Ix > Imax) { Ix = Imax; } else if (Ix < -1 * Imax) { Ix = -1 * Imax; } if (Iy > Imax) { Iy = Imax; } else if (Iy < -1 * Imax) { Iy = -1 * Imax; } if (Dx > Dmax) { Dx = Dmax; } else if (Dx < -1 * Dmax) { Dx = -1 * Dmax; } if (Dy > Dmax) { Dy = Dmax; } else if (Dy < -1 * Dmax) { Dy = -1 * Dmax; } /*************************************************************************** * Put all the terms together. Might want to look into pulling the K * values outside of the functions so they can just be multiplied * directly to the P, I, and D terms respectively ***************************************************************************/ Change23 = Px + Ix + Dx; Change34 = Py + Iy + Dy; // Change24 = Kp*errorZ + Ki*IntegralZ + Kd*DerivativeZ; /////////////////////////////////////////// // Change individual motor thrusts /////////////////////////////////////////// /* Over all form. Broken up to ensure total thrust remains 'constant' MotorThrusts[1-1] = MotorThrusts[1-1] - Change23 - Change34; //- Change24; MotorThrusts[2-1] = MotorThrusts[2-1] + Change23 - Change34; //+ Change24; MotorThrusts[3-1] = MotorThrusts[3-1] + Change23 + Change34; //- Change24; MotorThrusts[4-1] = MotorThrusts[4-1] - Change23 + Change34; //+ Change24; */ /*************************************************************************** * This following is just a whole lot of coding to ensure that the * 'total thrust' of all 4 motors remains a constant. This is done by * first adding the change in one direction, then seeing if that change * resulted in a value over the allowed max or under allowed min, then * correcting accordingly. Pretty sure this works as intended ***************************************************************************/ // x dimension changes (rotation about y (yaw)) MotorThrusts[1-1] = MotorThrusts[1-1] - Change23; MotorThrusts[2-1] = MotorThrusts[2-1] + Change23; MotorThrusts[3-1] = MotorThrusts[3-1] + Change23; MotorThrusts[4-1] = MotorThrusts[4-1] - Change23; // Check if change23 sends a motor below 0 if (MotorThrusts[2-1] < MotMin || MotorThrusts[3-1] < MotMin) { TempDel1 = MotMin - MotorThrusts[2-1]; TempDel2 = MotMin - MotorThrusts[3-1]; if (TempDel1 > TempDel2) { Delta = TempDel1; } else { Delta = TempDel2; } } else if (MotorThrusts[1-1] < MotMin || MotorThrusts[4-1] < MotMin) { TempDel1 = MotMin - MotorThrusts[1-1]; TempDel2 = MotMin - MotorThrusts[4-1]; if (TempDel1 > TempDel2) { Delta = -1*TempDel1; } else { Delta = -1*TempDel2; } } else { Delta = 0; } MotorThrusts[1-1] = MotorThrusts[1-1] - Delta; MotorThrusts[2-1] = MotorThrusts[2-1] + Delta; MotorThrusts[3-1] = MotorThrusts[3-1] + Delta; MotorThrusts[4-1] = MotorThrusts[4-1] - Delta; // Check if change sends motors above max if (MotorThrusts[2-1] > MotMax || MotorThrusts[3-1] > MotMax) { TempDel1 = MotMax - MotorThrusts[2-1]; TempDel2 = MotMax - MotorThrusts[3-1]; if (TempDel1 > TempDel2) { Delta = TempDel1; } else { Delta = TempDel2; } } else if (MotorThrusts[1-1] > MotMax || MotorThrusts[4-1] > MotMax) { TempDel1 = MotMax - MotorThrusts[1-1]; TempDel2 = MotMax - MotorThrusts[4-1]; if (TempDel1 > TempDel2) { Delta = -1*TempDel1; } else { Delta = -1*TempDel2; } } else { Delta = 0; } MotorThrusts[1-1] = MotorThrusts[1-1] - Delta; MotorThrusts[2-1] = MotorThrusts[2-1] + Delta; MotorThrusts[3-1] = MotorThrusts[3-1] + Delta; MotorThrusts[4-1] = MotorThrusts[4-1] - Delta; // y dimension changes (rotation about x (pitch)) MotorThrusts[1-1] = MotorThrusts[1-1] - Change34; MotorThrusts[2-1] = MotorThrusts[2-1] - Change34; MotorThrusts[3-1] = MotorThrusts[3-1] + Change34; MotorThrusts[4-1] = MotorThrusts[4-1] + Change34; // Check if change34 sends a motor below 0 if (MotorThrusts[3-1] < MotMin || MotorThrusts[4-1] < MotMin) { TempDel1 = MotMin - MotorThrusts[3-1]; TempDel2 = MotMin - MotorThrusts[4-1]; if (TempDel1 > TempDel2) { Delta = TempDel1; } else { Delta = TempDel2; } } else if (MotorThrusts[1-1] < MotMin || MotorThrusts[2-1] < MotMin) { TempDel1 = MotMin - MotorThrusts[1-1]; TempDel2 = MotMin - MotorThrusts[2-1]; if (TempDel1 > TempDel2) { Delta = -1*TempDel1; } else { Delta = -1*TempDel2; } } else { Delta = 0; } MotorThrusts[1-1] = MotorThrusts[1-1] - Delta; MotorThrusts[2-1] = MotorThrusts[2-1] - Delta; MotorThrusts[3-1] = MotorThrusts[3-1] + Delta; MotorThrusts[4-1] = MotorThrusts[4-1] + Delta; // Check if change sends motors above max if (MotorThrusts[3-1] > MotMax || MotorThrusts[4-1] > MotMax) { TempDel1 = MotMax - MotorThrusts[3-1]; TempDel2 = MotMax - MotorThrusts[4-1]; if (TempDel1 > TempDel2) { Delta = TempDel1; } else { Delta = TempDel2; } } else if (MotorThrusts[1-1] > MotMax || MotorThrusts[2-1] > MotMax) { TempDel1 = MotMax - MotorThrusts[1-1]; TempDel2 = MotMax - MotorThrusts[2-1]; if (TempDel1 > TempDel2) { Delta = -1*TempDel1; } else { Delta = -1*TempDel2; } } else { Delta = 0; } MotorThrusts[1-1] = MotorThrusts[1-1] - Delta; MotorThrusts[2-1] = MotorThrusts[2-1] - Delta; MotorThrusts[3-1] = MotorThrusts[3-1] + Delta; MotorThrusts[4-1] = MotorThrusts[4-1] + Delta; // Making sure it never goes above 100 or below 0 for (i = 0; i<4; i++) { if (MotorThrusts[i] > 100) { MotorThrusts[i] = 100; } if (MotorThrusts[i] < 0) { MotorThrusts[i] = 0; } } /////////////////////////////////// // Set Thrust values /////////////////////////////////// /* myputc('\n'); myputc('\r'); myputc('<'); myputi(MotorThrusts[1-1]); myputc(','); myputi(MotorThrusts[2-1]); myputc(','); myputi(MotorThrusts[3-1]); myputc(','); myputi(MotorThrusts[4-1]); myputc('>'); myputc('\n'); myputc('\r'); */ setPower(MotorThrusts[1-1], MotorThrusts[2-1], MotorThrusts[3-1], MotorThrusts[4-1]); } } }