Quadcopterv2 - This is for ICRS\' second generation Quadcopter

Users » madcowswe » Code » Quadcopterv2

Oskar Weigl / Mbed 2 deprecated Quadcopterv2

This is for ICRS\' second generation Quadcopter

main.cpp@0:0bbf2f16da9c, 2011-11-18 (annotated)

Committer:: madcowswe
Date:: Fri Nov 18 18:23:33 2011 +0000
Revision:: 0:0bbf2f16da9c

Who changed what in which revision?

User	Revision	Line number	New contents of line
madcowswe	0:0bbf2f16da9c	1	/*
madcowswe	0:0bbf2f16da9c	2	TODO: Organize functions n files
madcowswe	0:0bbf2f16da9c	3	TODO: Add Altsensor
madcowswe	0:0bbf2f16da9c	4	TODO: Make yaw from P to PID
madcowswe	0:0bbf2f16da9c	5	TODO: Make radiolink bidirectional
madcowswe	0:0bbf2f16da9c	6
madcowswe	0:0bbf2f16da9c	7	Positive X is forward, positive Y is left, positive Z is up!
madcowswe	0:0bbf2f16da9c	8
madcowswe	0:0bbf2f16da9c	9	Wii motion plus wireing!
madcowswe	0:0bbf2f16da9c	10	blue: gnd
madcowswe	0:0bbf2f16da9c	11	black: also gnd
madcowswe	0:0bbf2f16da9c	12	green: data
madcowswe	0:0bbf2f16da9c	13	brown: 3v3
madcowswe	0:0bbf2f16da9c	14	red: clk
madcowswe	0:0bbf2f16da9c	15
madcowswe	0:0bbf2f16da9c	16
madcowswe	0:0bbf2f16da9c	17	*/
madcowswe	0:0bbf2f16da9c	18
madcowswe	0:0bbf2f16da9c	19	#include "mbed.h"
madcowswe	0:0bbf2f16da9c	20	#include "GlobalsNDefines.h"
madcowswe	0:0bbf2f16da9c	21	#include <algorithm>
madcowswe	0:0bbf2f16da9c	22	#include "Sensors.h"
madcowswe	0:0bbf2f16da9c	23	#include "motors.h"
madcowswe	0:0bbf2f16da9c	24	#include "System.h"
madcowswe	0:0bbf2f16da9c	25	#include "RF12B.h"
madcowswe	0:0bbf2f16da9c	26	#include "main_init.h"
madcowswe	0:0bbf2f16da9c	27	#include <string>
madcowswe	0:0bbf2f16da9c	28
madcowswe	0:0bbf2f16da9c	29
madcowswe	0:0bbf2f16da9c	30	//Prototypes. TODO: Stick the functions into seperate files.
madcowswe	0:0bbf2f16da9c	31	void readcommands(float commands[5], float oldcommands[5]);
madcowswe	0:0bbf2f16da9c	32
madcowswe	0:0bbf2f16da9c	33	int main() {
madcowswe	0:0bbf2f16da9c	34
madcowswe	0:0bbf2f16da9c	35	//Initialize motors, sensors, etc.
madcowswe	0:0bbf2f16da9c	36	main_init();
madcowswe	0:0bbf2f16da9c	37
madcowswe	0:0bbf2f16da9c	38	//GOGOGO
madcowswe	0:0bbf2f16da9c	39	wait(0.5);
madcowswe	0:0bbf2f16da9c	40	pc.printf("Starting main control loop\r\n");
madcowswe	0:0bbf2f16da9c	41
madcowswe	0:0bbf2f16da9c	42	//Variables belonging to the control loop
madcowswe	0:0bbf2f16da9c	43	float G_roll_P = 55.0;//60.0;
madcowswe	0:0bbf2f16da9c	44	float G_roll_I = 46.0;
madcowswe	0:0bbf2f16da9c	45	float G_roll_D = 53.0;
madcowswe	0:0bbf2f16da9c	46	float G_pitch_P = 55.0;
madcowswe	0:0bbf2f16da9c	47	float G_pitch_I = 46.0;
madcowswe	0:0bbf2f16da9c	48	float G_pitch_D = 53.0;
madcowswe	0:0bbf2f16da9c	49	float G_yaw_P = 70.0;
madcowswe	0:0bbf2f16da9c	50	float rintegral = 0;
madcowswe	0:0bbf2f16da9c	51	float pintegral = 0;
madcowswe	0:0bbf2f16da9c	52	float yaw = 0;
madcowswe	0:0bbf2f16da9c	53	float pitch = 0;
madcowswe	0:0bbf2f16da9c	54	float roll = 0;
madcowswe	0:0bbf2f16da9c	55	float eroll = 0;
madcowswe	0:0bbf2f16da9c	56	float epitch = 0;
madcowswe	0:0bbf2f16da9c	57	float apitch = 0;
madcowswe	0:0bbf2f16da9c	58	float aroll = 0;
madcowswe	0:0bbf2f16da9c	59	float gyros[3];
madcowswe	0:0bbf2f16da9c	60	float depitch = 0;
madcowswe	0:0bbf2f16da9c	61	float deroll = 0;
madcowswe	0:0bbf2f16da9c	62	float altitude = 0.0; //Meters
madcowswe	0:0bbf2f16da9c	63	float commands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
madcowswe	0:0bbf2f16da9c	64	float oldcommands[5] = {0.0, 0.0, 0.0, 0.0, 0.0};
madcowswe	0:0bbf2f16da9c	65	signed char axisbuff[3];
madcowswe	0:0bbf2f16da9c	66
madcowswe	0:0bbf2f16da9c	67	//Last thing before entering loop: start the watchdog timer
madcowswe	0:0bbf2f16da9c	68	watchdog.attach(&wdt, 0.2);
madcowswe	0:0bbf2f16da9c	69
madcowswe	0:0bbf2f16da9c	70	while (true) {
madcowswe	0:0bbf2f16da9c	71
madcowswe	0:0bbf2f16da9c	72	//pc.printf("power is %f\r\n", (float));
madcowswe	0:0bbf2f16da9c	73	//pc.printf("yaw is %f, yawrate is %f\r\n", yaw, gyros[0]);
madcowswe	0:0bbf2f16da9c	74	//pc.printf("ptich is %f, pitchrate is %f\r\n", pitch, gyros[1]);
madcowswe	0:0bbf2f16da9c	75	//pc.printf("roll is %f, rollrate is %f\r\n", roll, gyros[2]);
madcowswe	0:0bbf2f16da9c	76	//pc.printf("\r\n");
madcowswe	0:0bbf2f16da9c	77
madcowswe	0:0bbf2f16da9c	78	//1 second inner loop to enable outer loop to print messages every 1s.
madcowswe	0:0bbf2f16da9c	79	//WARNING any delays, including printing, in the outer loop will deteriorate loop performace
madcowswe	0:0bbf2f16da9c	80	for (int looparound = 0; looparound < 100; looparound++) {
madcowswe	0:0bbf2f16da9c	81
madcowswe	0:0bbf2f16da9c	82	/*
madcowswe	0:0bbf2f16da9c	83	//Stop button kill
madcowswe	0:0bbf2f16da9c	84	if (!Nkill) {
madcowswe	0:0bbf2f16da9c	85	kill("Killed by E-stop button");
madcowswe	0:0bbf2f16da9c	86	}
madcowswe	0:0bbf2f16da9c	87	*/
madcowswe	0:0bbf2f16da9c	88
madcowswe	0:0bbf2f16da9c	89	/*
madcowswe	0:0bbf2f16da9c	90	//Commands from PC Serial
madcowswe	0:0bbf2f16da9c	91	if (pc.readable())
madcowswe	0:0bbf2f16da9c	92	readcommandstemp(commands, oldcommands);
madcowswe	0:0bbf2f16da9c	93	*/
madcowswe	0:0bbf2f16da9c	94
madcowswe	0:0bbf2f16da9c	95	//Commands from RF link
madcowswe	0:0bbf2f16da9c	96	if (radiolink.available() >= 5)
madcowswe	0:0bbf2f16da9c	97	readcommands(commands, oldcommands);
madcowswe	0:0bbf2f16da9c	98
madcowswe	0:0bbf2f16da9c	99	//Get sensor readings
madcowswe	0:0bbf2f16da9c	100	getaccel(axisbuff);
madcowswe	0:0bbf2f16da9c	101	getgyros(gyros);
madcowswe	0:0bbf2f16da9c	102	#ifdef ALTSENSOR
madcowswe	0:0bbf2f16da9c	103	altitude = getaltsensor();
madcowswe	0:0bbf2f16da9c	104	#endif
madcowswe	0:0bbf2f16da9c	105
madcowswe	0:0bbf2f16da9c	106	//One dimensional (small angle aproximation) linear to angular decoding
madcowswe	0:0bbf2f16da9c	107	apitch = 0.05 + atan2(-(float)axisbuff[0], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[1], 2) + pow((float)axisbuff[2], 2)));
madcowswe	0:0bbf2f16da9c	108	aroll = atan2(-(float)axisbuff[1], ((axisbuff[2] < 0) ? -1 : 1) * sqrt(pow((float)axisbuff[0], 2) + pow((float)axisbuff[2], 2)));
madcowswe	0:0bbf2f16da9c	109
madcowswe	0:0bbf2f16da9c	110	//low pass filter accelero and integrate gyros (note only 1 dimensional)
madcowswe	0:0bbf2f16da9c	111	pitch += (apitch - pitch) / ACCDECAY + gyros[1] * LOOPTIME;
madcowswe	0:0bbf2f16da9c	112	roll += (aroll - roll) / ACCDECAY + gyros[2] * LOOPTIME;
madcowswe	0:0bbf2f16da9c	113
madcowswe	0:0bbf2f16da9c	114	if (!integratestop){
madcowswe	0:0bbf2f16da9c	115	yaw += gyros[0] * LOOPTIME;
madcowswe	0:0bbf2f16da9c	116
madcowswe	0:0bbf2f16da9c	117	//Add yaw control (warning this approach will not work for navigation: makes the copter think it is always pointing north)
madcowswe	0:0bbf2f16da9c	118	yaw -= commands[3];
madcowswe	0:0bbf2f16da9c	119	}
madcowswe	0:0bbf2f16da9c	120
madcowswe	0:0bbf2f16da9c	121	//Errors
madcowswe	0:0bbf2f16da9c	122	epitch = pitch - commands[0];
madcowswe	0:0bbf2f16da9c	123	eroll = roll - commands[1];
madcowswe	0:0bbf2f16da9c	124
madcowswe	0:0bbf2f16da9c	125	//Error derivatives
madcowswe	0:0bbf2f16da9c	126	depitch = gyros[1] - (commands[0] - oldcommands[0]) / COMMANDPERIOD;
madcowswe	0:0bbf2f16da9c	127	deroll = gyros[2] - (commands[1] - oldcommands[1]) / COMMANDPERIOD;
madcowswe	0:0bbf2f16da9c	128
madcowswe	0:0bbf2f16da9c	129	//Average power to motors
madcowswe	0:0bbf2f16da9c	130	#ifdef ALTSENSOR
madcowswe	0:0bbf2f16da9c	131	float power = commands[2] - (altitude * ALTGAIN);
madcowswe	0:0bbf2f16da9c	132	#else
madcowswe	0:0bbf2f16da9c	133	float power = commands[2];
madcowswe	0:0bbf2f16da9c	134	#endif
madcowswe	0:0bbf2f16da9c	135
madcowswe	0:0bbf2f16da9c	136	/*
madcowswe	0:0bbf2f16da9c	137	//For some reason, the min function is not defined for float and float literal. Defining a temp float.
madcowswe	0:0bbf2f16da9c	138	float maxyawwtf = YAWCAP;
madcowswe	0:0bbf2f16da9c	139	SOLVED: use x.xf, regular = double..
madcowswe	0:0bbf2f16da9c	140	*/
madcowswe	0:0bbf2f16da9c	141
madcowswe	0:0bbf2f16da9c	142	//R+L corr term
madcowswe	0:0bbf2f16da9c	143	float LRcorr = eroll * G_roll_P
madcowswe	0:0bbf2f16da9c	144	+ rintegral * G_roll_I
madcowswe	0:0bbf2f16da9c	145	+ deroll * G_roll_D;
madcowswe	0:0bbf2f16da9c	146
madcowswe	0:0bbf2f16da9c	147	//F+R corr term
madcowswe	0:0bbf2f16da9c	148	float FRcorr = epitch * G_pitch_P
madcowswe	0:0bbf2f16da9c	149	+ pintegral * G_pitch_I
madcowswe	0:0bbf2f16da9c	150	+ depitch * G_pitch_D;
madcowswe	0:0bbf2f16da9c	151
madcowswe	0:0bbf2f16da9c	152	//yaw corr term
madcowswe	0:0bbf2f16da9c	153	float yawcorr = min(yaw, YAWCAP) * G_yaw_P;
madcowswe	0:0bbf2f16da9c	154
madcowswe	0:0bbf2f16da9c	155	// Setting motor speeds
madcowswe	0:0bbf2f16da9c	156	leftM(power + LRcorr - yawcorr);
madcowswe	0:0bbf2f16da9c	157	rightM(power - LRcorr - yawcorr);
madcowswe	0:0bbf2f16da9c	158	frontM(power + FRcorr + yawcorr);
madcowswe	0:0bbf2f16da9c	159	rearM(power - FRcorr + yawcorr);
madcowswe	0:0bbf2f16da9c	160
madcowswe	0:0bbf2f16da9c	161	//integrate
madcowswe	0:0bbf2f16da9c	162	if (!integratestop){
madcowswe	0:0bbf2f16da9c	163	rintegral += eroll * LOOPTIME;
madcowswe	0:0bbf2f16da9c	164	pintegral += epitch * LOOPTIME;
madcowswe	0:0bbf2f16da9c	165	}
madcowswe	0:0bbf2f16da9c	166
madcowswe	0:0bbf2f16da9c	167	//pet the dog
madcowswe	0:0bbf2f16da9c	168	loopalive = 1;
madcowswe	0:0bbf2f16da9c	169
madcowswe	0:0bbf2f16da9c	170	wait(LOOPTIME);
madcowswe	0:0bbf2f16da9c	171	}
madcowswe	0:0bbf2f16da9c	172	}
madcowswe	0:0bbf2f16da9c	173	}
madcowswe	0:0bbf2f16da9c	174
madcowswe	0:0bbf2f16da9c	175	void readcommands(float commands[5], float oldcommands[5]) {
madcowswe	0:0bbf2f16da9c	176	//pc.printf("We try to read commands\r\n");
madcowswe	0:0bbf2f16da9c	177	signed char data[5];
madcowswe	0:0bbf2f16da9c	178	radiolink.read((unsigned char*)data, 5);
madcowswe	0:0bbf2f16da9c	179
madcowswe	0:0bbf2f16da9c	180	for (int i = 0; i < 5; i++) {
madcowswe	0:0bbf2f16da9c	181	oldcommands[i] = commands[i];
madcowswe	0:0bbf2f16da9c	182
madcowswe	0:0bbf2f16da9c	183	switch (i) {
madcowswe	0:0bbf2f16da9c	184	case 0:
madcowswe	0:0bbf2f16da9c	185	commands[0] = data[i] * 0.0020;
madcowswe	0:0bbf2f16da9c	186	break;
madcowswe	0:0bbf2f16da9c	187	case 1:
madcowswe	0:0bbf2f16da9c	188	commands[1] = data[i] * -0.0020;
madcowswe	0:0bbf2f16da9c	189	break;
madcowswe	0:0bbf2f16da9c	190	case 2: {
madcowswe	0:0bbf2f16da9c	191	float throttle = (unsigned char)data[i];
madcowswe	0:0bbf2f16da9c	192	//commands[2] = (char)data[i] * 6.6 - (char)data[i] * (char)data[i] * 2.4; // / 5000.0;
madcowswe	0:0bbf2f16da9c	193	commands[2] = throttle * 5.0f - 0.01f * throttle * throttle;
madcowswe	0:0bbf2f16da9c	194	//pc.printf("throttle now at %f grams per motor\r\n", commands[2]);
madcowswe	0:0bbf2f16da9c	195	if (commands[2] < 250)
madcowswe	0:0bbf2f16da9c	196	integratestop = 1;
madcowswe	0:0bbf2f16da9c	197	else
madcowswe	0:0bbf2f16da9c	198	integratestop = 0;
madcowswe	0:0bbf2f16da9c	199	/*if (data[i] < -100) {
madcowswe	0:0bbf2f16da9c	200	kill("Killed by throttle low position");
madcowswe	0:0bbf2f16da9c	201	commands[4] = 1.0;
madcowswe	0:0bbf2f16da9c	202	}*/
madcowswe	0:0bbf2f16da9c	203	break;
madcowswe	0:0bbf2f16da9c	204	}
madcowswe	0:0bbf2f16da9c	205	case 3:
madcowswe	0:0bbf2f16da9c	206	commands[3] = data[i] * 0.0005;
madcowswe	0:0bbf2f16da9c	207	break;
madcowswe	0:0bbf2f16da9c	208	case 4:
madcowswe	0:0bbf2f16da9c	209	commands[4] = max((float)data[i], commands[4]);
madcowswe	0:0bbf2f16da9c	210	if (commands[4] > 0.1) {
madcowswe	0:0bbf2f16da9c	211	//pc.printf("Estopcommand was %f, last data was %d\r\n", commands[4], data[i]);
madcowswe	0:0bbf2f16da9c	212	kill("Killed by controller E-stop");
madcowswe	0:0bbf2f16da9c	213	}
madcowswe	0:0bbf2f16da9c	214	break;
madcowswe	0:0bbf2f16da9c	215	}
madcowswe	0:0bbf2f16da9c	216	}
madcowswe	0:0bbf2f16da9c	217
madcowswe	0:0bbf2f16da9c	218	commandsalive = 1;
madcowswe	0:0bbf2f16da9c	219	}
madcowswe	0:0bbf2f16da9c	220
madcowswe	0:0bbf2f16da9c	221
madcowswe	0:0bbf2f16da9c	222
madcowswe	0:0bbf2f16da9c	223
madcowswe	0:0bbf2f16da9c	224

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	18 Nov 2011
Imports:	30
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	1
Followers:	2

main.cpp@0:0bbf2f16da9c, 2011-11-18 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning