Oskar Weigl
/
Quadcopterv2
This is for ICRS\' second generation Quadcopter
main.cpp
- Committer:
- madcowswe
- Date:
- 2011-11-18
- Revision:
- 0:0bbf2f16da9c
File content as of revision 0:0bbf2f16da9c:
/* TODO: Organize functions n files 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 */ #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 = 55.0;//60.0; float G_roll_I = 46.0; float G_roll_D = 53.0; float G_pitch_P = 55.0; float G_pitch_I = 46.0; float G_pitch_D = 53.0; float G_yaw_P = 70.0; 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("ptich is %f, pitchrate is %f\r\n", pitch, gyros[1]); //pc.printf("roll is %f, rollrate is %f\r\n", roll, gyros[2]); //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))); //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; if (!integratestop){ 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 = 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; // Setting motor speeds leftM(power + LRcorr - yawcorr); rightM(power - LRcorr - yawcorr); frontM(power + FRcorr + yawcorr); rearM(power - FRcorr + yawcorr); //integrate if (!integratestop){ 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.0020; break; case 1: commands[1] = data[i] * -0.0020; break; case 2: { float throttle = (unsigned char)data[i]; //commands[2] = (char)data[i] * 6.6 - (char)data[i] * (char)data[i] * 2.4; // / 5000.0; commands[2] = throttle * 5.0f - 0.01f * throttle * throttle; //pc.printf("throttle now at %f grams per motor\r\n", commands[2]); if (commands[2] < 250) integratestop = 1; else integratestop = 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; }