Revision 31:872d8b8c7812, committed 2013-03-30
- Comitter:
- maetugr
- Date:
- Sat Mar 30 09:17:44 2013 +0000
- Parent:
- 30:021e13b62575
- Child:
- 32:e2e02338805e
- Commit message:
- new goals for the eastern holidays
Changed in this revision
--- a/IMU_Filter/IMU_Filter.cpp Sun Feb 10 22:08:10 2013 +0000
+++ b/IMU_Filter/IMU_Filter.cpp Sat Mar 30 09:17:44 2013 +0000
@@ -16,7 +16,7 @@
// Complementary Filter
#if 1 // (formula from http://diydrones.com/m/discussion?id=705844%3ATopic%3A669858)
angle[0] = (0.999*(angle[0] + d_Gyro_angle[0]))+(0.001*(Acc_angle[0]));
- angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(Acc_angle[1] + 3)); // TODO Offset accelerometer einstellen
+ angle[1] = (0.999*(angle[1] + d_Gyro_angle[1]))+(0.001*(Acc_angle[1]));// + 3)); // TODO Offset accelerometer einstellen
angle[2] += d_Gyro_angle[2]; // gyro only here TODO: Compass + 3D
#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LED.lib Sat Mar 30 09:17:44 2013 +0000
@@ -0,0 +1,1 @@
+LED#eb31b1718ac1
--- a/LED/LED.cpp Sun Feb 10 22:08:10 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,60 +0,0 @@
-#include "LED.h"
-#include "mbed.h"
-
-LED::LED() : Led(LED1, LED2, LED3, LED4){
- roller = 0;
-}
-
-void LED::shownumber(int number) {
- Led = number;
-}
-
-void LED::ride(int times = 1) {
- Led = 0;
- for (int j = 0; j < times; j++) {
- for(int i=0; i < 4; i++) {
- Led = 1 << i;
- wait(0.2);
- }
- }
- Led = 0;
-}
-
-void LED::roll(int times = 1) {
- Led = 0;
- for (int j = 0; j < (times*2); j++) {
- for(int roller = 1; roller <= 4; roller++) {
- tilt(roller);
- wait(0.1);
- }
- }
- roller = 0;
- Led = 0;
-}
-
-void LED::rollnext() {
- if (roller >= 4)
- roller = 0;
- roller++;
- tilt(roller);
-}
-
-void LED::tilt(int index) {
- Led = Led ^ (1 << (index-1)); //XOR
-}
-
-void LED::set(int index) {
- Led = Led | (1 << (index-1)); //OR
-}
-
-void LED::reset(int index) {
- Led = Led & ~(1 << (index-1)); //OR
-}
-
-int LED::check(int index) {
- return Led & (1 << (index-1));
-}
-
-void LED::operator=(int value) {
- Led = value;
-}
\ No newline at end of file
--- a/LED/LED.h Sun Feb 10 22:08:10 2013 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-// by MaEtUgR
-
-#ifndef LED_H
-#define LED_H
-
-#include "mbed.h"
-
-class LED {
-
- public:
- LED();
- void shownumber(int number);
- void ride(int times);
- void roll(int times);
- void rollnext();
- void tilt(int index);
- void set(int index);
- void reset(int index);
- int check(int index);
- void operator=(int value);
-
- private:
- BusOut Led;
- int roller;
-};
-
-#endif
\ No newline at end of file
--- a/main.cpp Sun Feb 10 22:08:10 2013 +0000
+++ b/main.cpp Sat Mar 30 09:17:44 2013 +0000
@@ -14,11 +14,12 @@
#define RATE 0.002 // speed of the interrupt for Sensors and PID
#define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
#define MAXPITCH 40 // maximal angle from horizontal that the PID is aming for
+#define RC_SENSITIVITY 20
#define YAWSPEED 2 // maximal speed of yaw rotation in degree per Rate
-#define P 0.35 // PID values
-#define I 0
-#define D 0.5
+float P = 1.5; // PID values
+float I = 0;
+float D = 1;
//#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
#define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC
@@ -53,8 +54,9 @@
unsigned long dt_read_sensors = 0;
unsigned long time_read_sensors = 0;
float tempangle = 0; // temporärer winkel für yaw mit kompass
-float controller_value[] = {0,0,0};
+float controller_value[] = {0,0,0}; // The calculated answer form the Controller
float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
+char command[300]; //= {'\0'};
void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
{
@@ -92,8 +94,9 @@
#endif
}
- for(int i=0;i<3;i++) // calculate new angle we want the QC to have
- RC_angle[i] = (RC[i].read()-500)*30/500.0;
+ for(int i=0;i<2;i++) // calculate new angle we want the QC to have
+ RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
+ //RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500;
for(int i=0;i<3;i++) {
Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
@@ -138,9 +141,37 @@
}
}
+void execute() {
+ if (command[0] == 'p')
+ P = atoi(&command[1]);
+ if (command[0] == 'i')
+ I = atoi(&command[1]);
+ if (command[0] == 'd')
+ D = atoi(&command[1]);
+}
+
+void pc_worker() {
+ char input = pc.getc();
+
+ if (input == '\r') {
+ execute();
+ command[0] = '\0';
+ } else {
+ int i = 0;
+ while(command[i] != '\0'){
+ i++;
+ LEDs.rollnext();
+ }
+ command[i] = input;
+ command[i+1] = '\0';
+ }
+}
+
int main() { // main programm for initialisation and debug output
NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished)
+ //pc.attach(&pc_worker); // zum Befehle geben
+
#ifdef LOGGER
Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
for(int i = 0; i < 3; i++) {
@@ -181,6 +212,8 @@
pc.printf("DIS_ARMED ");
pc.locate(5,3);
pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
+ pc.locate(5,4);
+ pc.printf("P:%6.1f I:%6.1f D:%6.1f ", P, I, D);
pc.locate(5,5);
pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
pc.locate(5,6);
@@ -200,6 +233,9 @@
pc.printf("RC1: %4d ", RC[1].read());
pc.printf("RC2: %4d ", RC[2].read());
pc.printf("RC3: %4d ", RC[3].read());
+
+ pc.locate(10,21);
+ pc.printf("Commandline: %s ", command);
#endif
if(armed){
LEDs.rollnext();
Matthias Grob
