Oskar Weigl
/
Quadcopter_copy
Very early flyable code.
main.cpp
- Committer:
- madcowswe
- Date:
- 2011-10-01
- Revision:
- 0:9fcb3bf5c231
File content as of revision 0:9fcb3bf5c231:
/* 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; }