Oskar Weigl
/
Quadcopter_copy
Very early flyable code.
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
diff -r 000000000000 -r 9fcb3bf5c231 GlobalsNDefines.h --- /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
diff -r 000000000000 -r 9fcb3bf5c231 RF12B.lib --- /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
diff -r 000000000000 -r 9fcb3bf5c231 Sensors.h --- /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
diff -r 000000000000 -r 9fcb3bf5c231 System.h --- /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; +}
diff -r 000000000000 -r 9fcb3bf5c231 main.cpp --- /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; +} + + + + +
diff -r 000000000000 -r 9fcb3bf5c231 main_init.h --- /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
diff -r 000000000000 -r 9fcb3bf5c231 mbed.bld --- /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
diff -r 000000000000 -r 9fcb3bf5c231 motors.h --- /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