Diff: quadv3/main.cpp

Revision:

0:978110f7f027

Child:

1:ac68f0368a77

diff -r 000000000000 -r 978110f7f027 quadv3/main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/quadv3/main.cpp	Wed Jan 30 13:14:44 2013 +0000
@@ -0,0 +1,260 @@
+/**
+ * @author Erik van de Coevering
+ *
+ * @section LICENSE
+ *
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge and/or publish copies of the Software, and to permit
+ * persons to whom the Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "mbed.h"
+#include "ADXL345.h"
+#include "ITG3200.h"
+#include "math.h"
+//#include "accelero.h"
+#include "bma180.h"
+#include "gyro.h"
+#include "IOMacros.h"
+#include "pwmout.h"
+#include "TextLCD.h"
+
+TextLCD lcd(p11, p12, p13, p14, p15, p16, TextLCD::LCD16x2);
+DigitalOut myled(LED1);
+
+Timer trudder;
+Timer tthrottle;
+Timer televator;
+Timer taileron;
+
+//Rx variables
+int ruddertime = 1616;
+int throttletime = 1100;
+int elevatortime = 850;
+int ailerontime = 850;
+int ruddercenter = 1616;
+int elevcenter = 1616;
+int aileroncenter = 1616;
+
+//Variables for calccomp
+float xcomp = 0;
+float ycomp = 0;
+int xcomp2 = 0;
+int ycomp2 = 0;
+float ruddercomp = 0;
+float ratio = 0.38;
+int ailer = 0;
+int elev = 0;
+int rud = 0;
+int zcomp = 0;
+int throttle = 0;
+float ailer2 = 0;
+float elev2 = 0;
+signed int m1 = 0;
+signed int m2 = 0;
+signed int m3 = 0;
+signed int m4 = 0;
+bool armed = false;
+float xsteering = 0;
+float ysteering = 0;
+
+DigitalIn rxthrottle(p25);
+DigitalIn rxaileron(p26);
+DigitalIn rxelevator(p29);
+DigitalIn rxrudder(p30);
+DigitalIn rxextra(p19);
+
+void calccomp(void)
+{
+    gyrosample();
+
+    //Scale rx channels
+    ailer = ailerontime - aileroncenter;
+    elev = elevatortime - elevcenter;
+    rud = ruddertime - ruddercenter;
+    throttle = throttletime;
+
+    xsteering = ailer;
+    ysteering = elev;
+    xsteering = xsteering/8;
+    ysteering = ysteering/8;
+    rud = rud/3;
+    throttle = ((throttle/2.2)+720);
+
+    //Start by mixing throttle
+    m1 = throttle;
+    m2 = throttle;
+    m3 = throttle;
+    m4 = throttle;
+
+    //Calc aileron compensation and mix with rx input
+    xcomp = (((((newax)-xsteering)*(1-ratio)) + ((yag+((newax-xsteering)*0.7))*ratio))*0.8);
+    xcomp2 = xcomp;
+
+    //Mix aileron
+    m1 = m1 - xcomp2;
+    m2 = m2 + xcomp2;
+    m3 = m3 + xcomp2;
+    m4 = m4 - xcomp2;
+
+    //Calc elevator compensation and mix with rx input
+    ycomp = (((((neway)+ysteering)*(1-ratio)) + ((xag+((neway+ysteering)*0.7))*ratio))*0.8);
+    ycomp2 = ycomp;
+
+    //Mix elevator
+    m1 = m1 - ycomp2;
+    m2 = m2 - ycomp2;
+    m3 = m3 + ycomp2;
+    m4 = m4 + ycomp2;
+
+    //Calc rudder compensation and mix with rx input
+    ruddercomp = (rud + (zarg*1));
+    zcomp = ruddercomp;
+
+    //Mix rudder
+    m1 = m1 - zcomp;
+    m2 = m2 + zcomp;
+    m3 = m3 - zcomp;
+    m4 = m4 + zcomp;
+
+    //Prevent motors from stalling
+//           if(m1 < 1075 && throttle > 1000) m1 = 1280;
+//           if(m2 < 1075 && throttle > 1000) m2 = 1280;
+//           if(m3 < 1075 && throttle > 1000) m3 = 1280;
+//           if(m4 < 1075 && throttle > 1000) m4 = 1280;
+
+    //When throttle down or if not armed, stop motors
+    if(throttle < 1350 || armed == false) {
+        m1=1200;
+        m2=1200;
+        m3=1200;
+        m4=1200;
+    }
+
+    //Stick arming
+    if(throttle < 1325 && rud > 120) {
+        armed = true;
+        myled = 1;
+    }
+    if(throttle < 1325 && rud < -105) {
+        armed = false;
+        myled = 0;
+    }
+
+    m3 += 20;
+
+    if(m1 > 1640) m1 = 1640;
+    if(m2 > 1640) m2 = 1640;
+    if(m3 > 1640) m3 = 1640;
+    if(m4 > 1640) m4 = 1640;
+
+    //Output to motors
+    setpwm(m1, m2, m3, m4);
+}
+
+
+
+int main()
+{
+    bool rudderflag = false;
+    bool throttleflag = false;
+    bool elevatorflag = false;
+    bool aileronflag = false;
+    bool extraflag = false;
+    bool exitflag = true;
+
+    int th = 0;
+    int ai = 0;
+    int el = 0;
+    int ru = 0;
+
+    myled = 0;
+
+    startpwm();
+    //  accstart();
+    acc1.init();
+    acc1.range(0);
+    acc1.bw(7);
+    gyrostart();
+    wait(1);
+    gyrocal();
+
+    while(1) {
+        if(!throttleflag && rxthrottle) {
+            tthrottle.start();
+            throttleflag = true;
+            extraflag = false;
+            calccomp();
+        }
+
+        if(!aileronflag && rxaileron) {
+            th = tthrottle.read_us();
+            if(th > 1000 && th < 2200) throttletime = th;
+            taileron.start();
+            tthrottle.reset();
+            aileronflag = true;
+            throttleflag = false;
+            calccomp();
+        }
+
+        if(!elevatorflag && rxelevator) {
+            ai = taileron.read_us();
+            if(ai > 1000 && ai < 2200) ailerontime = ai;
+            televator.start();
+            taileron.reset();
+            elevatorflag = true;
+            aileronflag = false;
+            calccomp();
+        }
+
+        if(!rudderflag && rxrudder) {
+            el = televator.read_us();
+            if(el > 1000 && el < 2200) elevatortime = el;
+            trudder.start();
+            televator.reset();
+            rudderflag = true;
+            elevatorflag = false;
+            calccomp();
+        }
+
+        if(!extraflag && rxextra) {
+            ru = trudder.read_us();
+            if(ru > 1000 && ru < 2200) ruddertime = ru;
+            trudder.reset();
+            extraflag = true;
+            rudderflag = false;
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+            wait_us(500);
+            calccomp();
+        }
+    }
+}
+