Revision 0:9fcb3bf5c231, committed 2011-10-01

Comitter:

madcowswe

Date:

Sat Oct 01 12:57:23 2011 +0000

Commit message:

This edit is for testing: not flyable

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/GlobalsNDefines.h	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,40 @@
+/*
+#ifndef GlobalsNDefines_H_
+#define GlobalsNDefines_H_
+*/
+
+#include "mbed.h"
+#include "RF12B.h"
+
+Serial pc(USBTX, USBRX); // tx, rx
+I2C i2c(p28, p27);
+//Rfm12b rfm12b(p5, p6, p7, p8, p11);
+RF12B radiolink(p5, p6, p7, p8, p9);
+DigitalOut myled(LED1);
+DigitalOut motormaxled(LED2);
+DigitalIn Nkill(p10);
+Ticker watchdog;
+
+PwmOut PWMfront(p23);
+PwmOut PWMrear(p22);
+PwmOut PWMleft(p24);
+PwmOut PWMright(p21);
+
+AnalogIn altsensor(p15);
+
+#define RPSPUNIT -0.00126766
+#define NOPOWER 0.063
+#define ACCADDRESS 0x4C
+#define LOOPTIME 0.01
+#define ACCDECAY 50
+#define YAWCAP 1.5f
+#define COMMANDPERIOD 0.1;
+#define ALTGAIN 
+//#define ALTSENSOR
+
+float gyrcalib[] = {0.0, 0.0, 0.0};
+bool loopalive = 1;
+bool commandsalive = 1;
+
+
+//#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RF12B.lib	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/harryeakins/code/RF12B/#cd34784da6da

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors.h	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,98 @@
+
+//In here be functions related to Sensors
+
+//
+void setupaccel() {
+    i2c.start();
+    i2c.write(ACCADDRESS << 1 | 0);
+    i2c.write(7);
+    i2c.write(1);
+    i2c.stop();
+}
+
+//
+void getaccel(signed char returnbuff[3]) {
+    i2c.start();
+    i2c.write(ACCADDRESS << 1 | 0);
+    i2c.write(0);
+    i2c.stop();
+
+    //Read the axis data
+    i2c.read(ACCADDRESS << 1 | 1, (char*)returnbuff, 3);
+
+    for (int i = 0; i < 3; i++) {
+        returnbuff[i] = returnbuff[i] << 2;
+        returnbuff[i] /= 4;
+    }
+
+}
+
+void calibgyro() {
+
+    //i2c.frequency(9600);
+
+    //activating the wiimotion +
+    i2c.start();
+    //device address | write
+    i2c.write(0x53 << 1 | 0);
+    //register address
+    i2c.write(0xfe);
+    //data
+    i2c.write(0x04);
+    i2c.stop();
+
+    for (int i = 0; i < 10; i++) {
+        //sending a 0x00 to flag that we want data
+        i2c.start();
+        //device address | write (note new address)
+        i2c.write(0x52 << 1 | 0);
+        //send 0x00
+        i2c.write(0x00);
+        i2c.stop();
+
+        //reading the data
+        char wmpdata[6];
+        i2c.read(0x52 << 1 | 1, wmpdata, 6);
+        pc.printf("%x %x %x %x %x %x\r\n",  wmpdata[0], wmpdata[1], wmpdata[2], wmpdata[3], wmpdata[4], wmpdata[5]);
+
+        gyrcalib[0] += (((wmpdata[3] >> 2) << 8) + wmpdata[0]) / 10;
+        gyrcalib[1] += (((wmpdata[4] >> 2) << 8) + wmpdata[1]) / 10;
+        gyrcalib[2] += (((wmpdata[5] >> 2) << 8) + wmpdata[2]) / 10;
+
+        wait(0.05);
+
+    }
+
+}
+
+void getgyros(float gyrodata[3]) {
+    //sending a 0x00 to flag that we want data
+    i2c.start();
+    //device address | write (note new address)
+    i2c.write(0x52 << 1 | 0);
+    //send 0x00
+    i2c.write(0x00);
+    i2c.stop();
+
+    //reading the data
+    char wmpdata[6];
+    i2c.read(0x52 << 1 | 1, wmpdata, 6);
+
+    gyrodata[0] = RPSPUNIT * -(((wmpdata[3] >> 2) << 8) + wmpdata[0] - gyrcalib[0]);
+    gyrodata[1] = RPSPUNIT * (((wmpdata[4] >> 2) << 8) + wmpdata[1] - gyrcalib[1]);
+    gyrodata[2] = RPSPUNIT * (((wmpdata[5] >> 2) << 8) + wmpdata[2] - gyrcalib[2]);
+
+    //detect if we ever went into fast mode
+    if (!(wmpdata[3] & 0x02 && wmpdata[4] & 0x02 && wmpdata[5] & 0x02)) myled = 1;
+
+    //pc.printf("yaw: %f, pitch: %f, roll: %f\r\n", gyrodata[0], gyrodata[1], gyrodata[2]);
+
+    //wait(0.05);
+
+}
+
+#ifdef ALTSENSOR
+float getaltsensor() {
+    return altsensor * 
+}
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/System.h	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,32 @@
+
+//General system functions
+
+#include <string>
+
+//
+void kill(string reason = "Killed for no reason!") {
+    PWMleft = NOPOWER - 0.004;
+    PWMright = NOPOWER - 0.004;
+    PWMfront = NOPOWER - 0.004;
+    PWMrear = NOPOWER - 0.004;
+    //TODO: send this over RF as well..
+    pc.printf("%s\r\n", reason.c_str());
+    /*wait(0.5);
+    left = 0;
+    right = 0;
+    front = 0;
+    rear = 0;
+    */
+    while (1);
+}
+
+//
+void wdt() {
+    if (!loopalive)
+        kill("Killed by WDT: loop has stalled");
+    if (!commandsalive)
+        kill("Killed by WDT: command starvation");
+
+    loopalive = 0;
+    commandsalive = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,282 @@
+/*
+TODO: Organize functions n files
+TODO: move hardcoded values to defines
+TODO: Add Altsensor
+TODO: Make yaw from P to PID
+TODO: Make radiolink bidirectional
+
+Positive X is forward, positive Y is left, positive Z is up!
+
+Wii motion plus wireing!
+blue: gnd
+black: also gnd
+green: data
+brown: 3v3
+red: clk
+
+critical gain = NOPOWER / 50
+period = 30/11s
+
+*/
+
+#include "mbed.h"
+#include "GlobalsNDefines.h"
+#include <algorithm>
+#include "Sensors.h"
+#include "motors.h"
+#include "System.h"
+#include "RF12B.h"
+#include "main_init.h"
+#include <string>
+
+
+//Prototypes. TODO: Stick the functions into seperate files.
+void readcommands(float commands[5], float oldcommands[5]);
+
+int main() {
+
+    //Initialize motors, sensors, etc.
+    main_init();
+
+    //GOGOGO
+    wait(0.5);
+    pc.printf("Starting main control loop\r\n");
+
+    //Variables belonging to the control loop
+    float G_roll_P = 35.65;
+    float G_roll_I = 35.65;
+    float G_roll_D = 22.28;
+    float G_pitch_P = 35.65;
+    float G_pitch_I = 35.65;
+    float G_pitch_D = 22.28;
+    float G_yaw_P = 31.20;
+    float rintegral = 0;
+    float pintegral = 0;
+    float yaw = 0;
+    float pitch = 0;
+    float roll = 0;
+    float eroll = 0;
+    float epitch = 0;
+    float apitch = 0;
+    float aroll = 0;
+    float gyros[3];
+    float depitch = 0;
+    float deroll = 0;
+    float altitude = 0.0; //Meters
+    float commands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
+    float oldcommands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
+    signed char axisbuff[3];
+
+    //Last thing before entering loop: start the watchdog timer
+    //watchdog.attach(&wdt, 0.2);
+
+    while (true) {
+
+        //pc.printf("power is %f\r\n", (float));
+        pc.printf("yaw is %f, yawrate is %f\r\n", yaw, gyros[0]);
+        pc.printf("\r\n");
+
+        //1 second inner loop to enable outer loop to print messages every 1s.
+        //WARNING any delays, including printing, in the outer loop will deteriorate loop performace
+        for (int looparound = 0; looparound < 100; looparound++) {
+
+            /*
+            //Stop button kill
+            if (!Nkill) {
+                kill("Killed by E-stop button");
+            }
+            */
+
+            /*
+            //Commands from PC Serial
+            if (pc.readable())
+                readcommandstemp(commands, oldcommands);
+            */
+
+            //Commands from RF link
+            if (radiolink.available() >= 5)
+                readcommands(commands, oldcommands);
+
+            //Get sensor readings
+            getaccel(axisbuff);
+            getgyros(gyros);
+#ifdef ALTSENSOR
+            altitude = getaltsensor();
+#endif
+
+            //One dimensional (small angle aproximation) linear to angular decoding
+            apitch = 0.05 + atan2(-(float)axisbuff[0], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[1], 2) + pow((float)axisbuff[2], 2)));
+            aroll = atan2(-(float)axisbuff[1], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[0], 2) + pow((float)axisbuff[2], 2)));
+
+            //try a different trim (hack) WARNING THIS IS SLOW!
+            //apitch -= commands[0];
+            //aroll -= commands[1];
+
+            //low pass filter accelero and integrate gyros (note only 1 dimensional)
+            pitch += (apitch - pitch) / ACCDECAY + gyros[1] * LOOPTIME;
+            roll += (aroll - roll) / ACCDECAY + gyros[2] * LOOPTIME;
+            yaw += gyros[0] * LOOPTIME;
+
+            //Add yaw control (warning this approach will not work for navigation: makes the copter think it is always pointing north)
+            yaw -= commands[3];
+
+            //Errors
+            epitch = pitch - commands[0];
+            eroll = roll - commands[1];
+
+            //Error derivatives
+            depitch = gyros[1] - (commands[0] - oldcommands[0]) / COMMANDPERIOD;
+            deroll = gyros[2] - (commands[1] - oldcommands[1]) / COMMANDPERIOD;
+
+            /*
+            //Average power to motors
+            #ifdef ALTSENSOR
+            float power = NOPOWER - 0.002 + commands[2] - (altitude * ALTGAIN);
+            #else
+            float power = *//*pot / 10*//* NOPOWER - 0.002 + commands[2];
+#endif
+*/
+
+            //Average power to motors
+#ifdef ALTSENSOR
+            float power = commands[2] - (altitude * ALTGAIN);
+#else
+            float power = commands[2];
+#endif
+
+            /*
+            //For some reason, the min function is not defined for float and float literal. Defining a temp float.
+            float maxyawwtf = YAWCAP;
+            SOLVED: use x.xf, regular = double..
+            */
+
+            //R+L corr term
+            float LRcorr = eroll * G_roll_P
+                           + rintegral * G_roll_I
+                           + deroll * G_roll_D;
+
+            //F+R corr term
+            float FRcorr = epitch * G_pitch_P
+                           + pintegral * G_pitch_I
+                           + depitch * G_pitch_D;
+
+            //yaw corr term
+            float yawcorr = min(yaw, YAWCAP) * G_yaw_P;
+
+            //left pid
+            motormaxled = motormaxled || leftM(power - LRcorr - yawcorr);
+
+            //right pid
+            motormaxled = motormaxled || rightM(power + LRcorr - yawcorr);
+
+            //front pid
+            motormaxled = motormaxled || frontM(power - FRcorr + yawcorr);
+
+            //rear pid
+            motormaxled = motormaxled || rearM(power + FRcorr + yawcorr);
+
+
+            /*
+            //left pid
+            left = min(max(power - (
+
+                               eroll * (0.8 * NOPOWER / 25)
+                               + rintegral * (0.8 * NOPOWER / 25)
+                               + deroll * (0.5 * NOPOWER / 25)
+                           ) - (
+                               min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
+
+                           ), 0.060), NOPOWER * 1.5);
+
+
+            //right pid
+            right = min(max(power + (
+
+                                eroll * (0.8 * NOPOWER / 25)
+                                + rintegral * (0.8 * NOPOWER / 25)
+                                + deroll * (0.5 * NOPOWER / 25)
+                            ) - (
+                                min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
+
+                            ), 0.060), NOPOWER * 1.5);
+
+
+            //front pid
+            front = min(max(power - (
+
+                                epitch * (0.8 * NOPOWER / 25)
+                                + pintegral * (0.8 * NOPOWER / 25)
+                                + depitch * (0.5 * NOPOWER / 25)
+                            ) + (
+                                min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
+
+                            ), 0.060), NOPOWER * 1.5);
+
+
+            //rear pid
+            rear = min(max(power + (
+
+                               epitch * (0.8 * NOPOWER / 25)
+                               + pintegral * (0.8 * NOPOWER / 25)
+                               + depitch * (0.5 * NOPOWER / 25)
+                           ) + (
+                               min(yaw, maxyawwtf) * (0.7 * NOPOWER / 25)
+
+                           ), 0.060), NOPOWER * 1.5);
+
+            */
+
+            //integrate
+            rintegral += eroll * LOOPTIME;
+            pintegral += epitch * LOOPTIME;
+
+            //pet the dog
+            loopalive = 1;
+
+            wait(LOOPTIME);
+        }
+    }
+}
+
+void readcommands(float commands[5], float oldcommands[5]) {
+    //pc.printf("We try to read commands\r\n");
+    signed char data[5];
+    radiolink.read((unsigned char*)data, 5);
+
+    for (int i = 0; i < 5; i++) {
+        oldcommands[i] = commands[i];
+
+        switch (i) {
+            case 0:
+                commands[0] = data[i] * -0.0014;
+                break;
+            case 1:
+                commands[1] = data[i] * 0.0014;
+                break;
+            case 2:
+                commands[2] = data[i] * 2.8; // / 5000.0;
+                /*if (data[i] < -100) {
+                    kill("Killed by throttle low position");
+                    commands[4] = 1.0;
+                }*/
+                break;
+            case 3:
+                commands[3] = data[i] * 0.0005;
+                break;
+            case 4:
+                commands[4] = max((float)data[i], commands[4]);
+                if (commands[4] > 0.1) {
+                    //pc.printf("Estopcommand was %f, last data was %d\r\n", commands[4], data[i]);
+                    kill("Killed by controller E-stop");
+                }
+                break;
+        }
+    }
+
+    commandsalive = 1;
+}
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_init.h	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,36 @@
+
+
+void main_init() {
+
+    //Emergency stop button
+    Nkill.mode(PullDown);
+
+    pc.printf("Hello World!\r\n");
+
+    initmotor();
+
+    /*
+    //Init motor pwm
+    left.period_ms(20);
+    left = 0.01;
+    right.period_ms(20);
+    right = 0.01;
+
+    front.period_ms(20);
+    front = 0.01;
+    rear.period_ms(20);
+    rear = 0.01;
+    */
+
+    pc.printf("Motors initialized\r\n");
+
+    //Setup accelerometer
+    //setupaccel();
+    //pc.printf("Accelorometer initialized\r\n");
+
+    //Setup gyros, and calibrate them
+    wait(0.5);
+    calibgyro();
+    pc.printf("Gyros initialized and calibrated\r\n");
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motors.h	Sat Oct 01 12:57:23 2011 +0000
@@ -0,0 +1,56 @@
+
+void initmotor() {
+    PWMleft.period_ms(20);
+    PWMleft = 0.01;
+    PWMright.period_ms(20);
+    PWMright = 0.01;
+
+    PWMfront.period_ms(20);
+    PWMfront = 0.01;
+    PWMrear.period_ms(20);
+    PWMrear = 0.01;
+}
+
+bool leftM(float thrust) {
+    float output = (634 + 0.657 * thrust) / 10000;
+    if (output > 0.0910) {
+        PWMleft = 0.0910;
+        return true;
+    } else {
+        PWMleft = output;
+        return false;
+    }
+}
+
+bool rightM(float thrust) {
+    float output = (617 + 0.474 * thrust) / 10000;
+        if (output > 0.0822) {
+        PWMright = 0.0822;
+        return true;
+    } else {
+        PWMright = output;
+        return false;
+    }
+}
+
+bool frontM(float thrust) {
+    float output = (637 + 0.509 * thrust) / 10000;
+        if (output > 0.0880) {
+        PWMfront = 0.0800;
+        return true;
+    } else {
+        PWMfront = output;
+        return false;
+    }
+}
+
+bool rearM(float thrust) {
+    float output = (644 + 0.470 * thrust) / 10000;
+        if (output > 0.0908) {
+        PWMrear = 0.0908;
+        return true;
+    } else {
+        PWMrear = output;
+        return false;
+    }
+}
\ No newline at end of file