Revision 15:753c5d6a63b3, committed 2012-10-29

Comitter:

maetugr

Date:

Mon Oct 29 16:43:10 2012 +0000

Parent:

14:cf260677ecde

Child:

16:74a6531350b5

Commit message:

I2C/Interrupt-Problem gel?st!! erste Versuche an gespannter schnur, falsche PID werte mitten im umschreiben auf I2C_Sensor mutterklasse

Changed in this revision

--- a/RC/RC_Channel.cpp	Sat Oct 27 10:53:43 2012 +0000
+++ b/RC/RC_Channel.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -6,7 +6,7 @@
     time = -1; // start value to see if there was any value yet
     myinterrupt.rise(this, &RC_Channel::rise);
     myinterrupt.fall(this, &RC_Channel::fall);
-    //timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
+    timeoutchecker.attach(this, &RC_Channel::timeoutcheck, 1);
     
 }
 
@@ -18,13 +18,11 @@
 void RC_Channel::rise()
 {
     timer.start();
-    LEDs.set(2);
 }
 
 void RC_Channel::fall()
 {
     timer.stop();
-    LEDs.reset(2);
     int tester = timer.read_us();
     if(tester >= 1000 && tester <=2000)
         time = tester;

--- a/RC/RC_Channel.h	Sat Oct 27 10:53:43 2012 +0000
+++ b/RC/RC_Channel.h	Mon Oct 29 16:43:10 2012 +0000
@@ -2,7 +2,6 @@
 #define RC_CHANNEL_H
 
 #include "mbed.h"
-#include "LED.h"
 
 class RC_Channel
 {
@@ -17,8 +16,6 @@
         Timer timer; // timer to measure the up time of the signal and if the signal timed out
         int time; // last measurement data
         
-        LED LEDs;
-        
         Ticker timeoutchecker; // Ticker to see if signal broke down
         void timeoutcheck(); // to check for timeout, checked every second
 };

--- a/Sensors/Acc/ADXL345.cpp	Sat Oct 27 10:53:43 2012 +0000
+++ b/Sensors/Acc/ADXL345.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -76,8 +76,8 @@
 }
 
 void ADXL345::readMultiReg(char address, char* output, int size) {
-    i2c.write(ADXL345_WRITE, &address, 1); //tell it where to read from
-    i2c.read(ADXL345_READ , output, size); //tell it where to store the data read
+    i2c.write(ADXL345_WRITE, &address, 1, true); //tell it where to read from
+    i2c.read(ADXL345_READ , output, size, true); //tell it where to store the data read
 }
 
 void ADXL345::setDataRate(char rate) {

--- a/Sensors/Comp/HMC5883.cpp	Sat Oct 27 10:53:43 2012 +0000
+++ b/Sensors/Comp/HMC5883.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -94,8 +94,8 @@
 }
 
 void HMC5883::readMultiReg(char address, char* output, int size) {
-    i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1); //tell it where to read from
-    i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size); //tell it where to store the data read
+    i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1, true); //tell it where to read from
+    i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size, true); //tell it where to store the data read
 }
 
 float HMC5883::get_angle()

--- a/Sensors/Gyro/L3G4200D.cpp	Sat Oct 27 10:53:43 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -7,7 +7,7 @@
 
 L3G4200D::L3G4200D(PinName sda, PinName scl) : i2c(sda, scl)
 {
-    i2c.frequency(100000);
+    i2c.frequency(400000);
     // Turns on the L3G4200D's gyro and places it in normal mode.
     // Normal power mode, all axes enabled
     
@@ -78,28 +78,10 @@
     // to do slave-transmit subaddress updating.
     buffer[0] = L3G4200D_OUT_X_L | (1 << 7);
     
-    //i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1); 
-    
-    //i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6); 
-    
-    L3G4200D_OUT_X_L
-    L3G4200D_OUT_X_H
-    L3G4200D_OUT_Y_L
-    L3G4200D_OUT_Y_H
-    L3G4200D_OUT_Z_L
-    L3G4200D_OUT_Z_H
+    i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1, true); 
+    i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6, true); 
     
-    i2c.start();
-    i2c.write(L3G4200D_I2C_ADDRESS);
-    i2c.write(L3G4200D_OUT_X_L);
-
-    i2c.start();
-    i2c.write(L3G4200D_I2C_ADDRESS | 1);
-    buffer[1]  = i2c.read(1) << 8;
-    buffer[1] |= i2c.read(0);
-    i2c.stop();
-    
-    //data[0] = (short) (buffer[1] << 8 | buffer[0]);
+    data[0] = (short) (buffer[1] << 8 | buffer[0]);
     data[1] = (short) (buffer[3] << 8 | buffer[2]);
     data[2] = (short) (buffer[5] << 8 | buffer[4]);
     

--- a/Sensors/I2C_Sensor.cpp	Sat Oct 27 10:53:43 2012 +0000
+++ b/Sensors/I2C_Sensor.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -12,7 +12,7 @@
     i2c.frequency(1500000); // ultrafast!
 }
 
-void saveCalibrationValues(float values[], int size, char * filename)
+void I2C_Sensor::saveCalibrationValues(float values[], int size, char * filename)
 {
     FILE *fp = fopen(strcat("/local/", filename), "w");
     for(int i = 0; i < size; i++)
@@ -20,7 +20,7 @@
     fclose(fp);
 }
 
-void loadCalibrationValues(float values[], int size, char * filename)
+void I2C_Sensor::loadCalibrationValues(float values[], int size, char * filename)
 {
     FILE *fp = fopen(strcat("/local/", filename), "r");
     for(int i = 0; i < size; i++)
@@ -28,14 +28,26 @@
     fclose(fp);
 }
 
-void I2C_Sensor::writeRegister(char address, char data){ 
-    char tx[2];
-    tx[0] = address;
-    tx[1] = data;
-    i2c.write(GET_I2C_WRITE_ADDRESS(i2c_address), tx, 2);
+char I2C_Sensor::readRegister(char reg)
+{
+    char value = 0;
+    
+    i2c.write(L3G4200D_I2C_ADDRESS, &reg, 1, true);
+    i2c.read(L3G4200D_I2C_ADDRESS, &value, 1, true);
+
+    return value;
 }
 
-void I2C_Sensor::readMultiRegister(char address, char* output, int size) {
-    i2c.write (GET_I2C_WRITE_ADDRESS(i2c_address), &address, 1);      //tell it from which register to read
-    i2c.read  (GET_I2C_READ_ADDRESS(i2c_address), output, size);      //tell it where to store the data read
+void I2C_Sensor::writeRegister(char reg, char data)
+{ 
+    char buffer[2]; // TODO: Arraykonstruktion in eine Zeile
+    buffer[0] = reg;
+    buffer[1] = data;
+    i2c.write(GET_I2C_WRITE_ADDRESS(i2c_address), tx, 2, true);
+}
+
+void I2C_Sensor::readMultiRegister(char reg, char* output, int size)
+{ // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes (see http://www.i2c-bus.org/repeated-start-condition/)
+    i2c.write (GET_I2C_WRITE_ADDRESS(i2c_address), &reg, 1, true);      //tell it from which register to read
+    i2c.read  (GET_I2C_READ_ADDRESS(i2c_address), output, size, true);      //tell it where to store the data read
 }
\ No newline at end of file

--- a/Sensors/I2C_Sensor.h	Sat Oct 27 10:53:43 2012 +0000
+++ b/Sensors/I2C_Sensor.h	Mon Oct 29 16:43:10 2012 +0000
@@ -12,18 +12,18 @@
         
     protected:
         // I2C functions
-        void writeRegister(char address, char data);
-        void readMultiRegister(char address, char* output, int size);
-        
-    private:
-        I2C i2c;            // I2C-Bus
-    
-        int8_t i2c_address; // address
+        char readRegister(char reg);
+        void writeRegister(char reg, char data);
+        void readMultiRegister(char reg, char* output, int size);
         
         // raw data and function to measure it
         int raw[3];
         void readraw();
         
+    private:
+        I2C i2c;            // I2C-Bus
+        int8_t i2c_address; // address
+        
         LocalFileSystem local; // file access to save calibration values
 };
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo_PWM/Servo_PWM.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -0,0 +1,23 @@
+#include "Servo_PWM.h"
+#include "mbed.h"
+
+Servo_PWM::Servo_PWM(PinName Pin) : ServoPin(Pin) {
+    ServoPin.period(0.020);
+    ServoPin = 0;
+    initialize(); // TODO: Works?
+}
+
+void Servo_PWM::initialize() {
+    // initialize ESC
+    SetPosition(1000); // full throttle
+    wait(0.01);         // for 0.01 secs
+    SetPosition(1000);  // low throttle
+}
+
+void Servo_PWM::SetPosition(int position) {
+    ServoPin.pulsewidth(position/1000000.0);
+}
+
+void Servo_PWM::operator=(int position) {
+    SetPosition(position);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo_PWM/Servo_PWM.h	Mon Oct 29 16:43:10 2012 +0000
@@ -0,0 +1,59 @@
+// by MaEtUgR
+
+#ifndef SERVO_PWM_H
+#define SERVO_PWM_H
+
+#include "mbed.h"
+
+/** Class to control a servo on any pin, without using pwm
+ *
+ * Example:
+ * @code
+ * // Keep sweeping servo from left to right
+ * #include "mbed.h"
+ * #include "Servo.h"
+ * 
+ * Servo Servo1(p20);
+ *
+ * Servo1.Enable(1500,20000);
+ *
+ * while(1) {
+ *     for (int pos = 1000; pos < 2000; pos += 25) {
+ *         Servo1.SetPosition(pos);  
+ *         wait_ms(20);
+ *     }
+ *     for (int pos = 2000; pos > 1000; pos -= 25) {
+ *         Servo1.SetPosition(pos); 
+ *         wait_ms(20); 
+ *     }
+ * }
+ * @endcode
+ */
+
+class Servo_PWM {
+
+public:
+    /** Create a new Servo object on any mbed pin
+     *
+     * @param Pin Pin on mbed to connect servo to
+     */
+    Servo_PWM(PinName Pin);
+    
+    /** Change the position of the servo. Position in us
+     *
+     * @param NewPos The new value of the servos position (us)
+     */
+    void SetPosition(int position);
+    
+    //operator for confortable positioning
+    void operator=(int position);
+    
+    // initialize ESC
+    void initialize();
+
+private:
+    PwmOut ServoPin;
+    
+};
+
+#endif
\ No newline at end of file

--- a/main.cpp	Sat Oct 27 10:53:43 2012 +0000
+++ b/main.cpp	Mon Oct 29 16:43:10 2012 +0000
@@ -6,13 +6,17 @@
 #include "HMC5883.h"    // Comp (Compass)
 #include "BMP085_old.h"     // Alt (Altitude sensor)
 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
-#include "Servo.h"      // Motor PPM
+#include "Servo_PWM.h"  // Motor PPM using PwmOut
 #include "PID.h"        // PID Library by Aaron Berk
 #include "IntCtrl.h"    // Interrupt Control by Roland Elmiger
 
 #define PI              3.1415926535897932384626433832795   // ratio of a circle's circumference to its diameter
 #define Rad2Deg         57.295779513082320876798154814105   // ratio between radians and degree (360/2Pi)
-#define RATE            0.0007                               // speed of the interrupt for Sensors and PID
+#define RATE            0.02                                // speed of the interrupt for Sensors and PID
+
+#define P_VALUE         0.1                                // viel zu tief!!!!!
+#define I_VALUE         0.0                                // 
+#define D_VALUE         0.0                                // 
 
 //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start
 
@@ -26,12 +30,12 @@
 ADXL345     Acc(p28, p27);
 HMC5883     Comp(p28, p27);
 BMP085_old      Alt(p28, p27);
-RC_Channel  RC[] = {(p11), (p12), (p13), (p14)}; // noooo p19/p20!!!!
-Servo       Motor[] = {(p15), (p16), (p17), (p18)};
+RC_Channel  RC[] = {(p11), (p12), (p13), (p14)};    // noooo p19/p20 !
+Servo_PWM   Motor[] = {(p21), (p22), (p23), (p24)}; // p21 - p26 only !
 
-PID controller(1.0, 0.0, 0.0, RATE);  // test PID controller for throttle
-PID pid(1.0, 1.0, 0.0, RATE);         // test PID controller for throttle
-//PID P:3,0 bis 3,5 I:0,010 und 0,050 D:5 und 25 
+//PID         Controller[] = {(P_VALUE, I_VALUE, D_VALUE, RATE), (P_VALUE, I_VALUE, D_VALUE, RATE), (P_VALUE, I_VALUE, D_VALUE, RATE)}; // TODO: RATE != dt immer anpassen
+//PID       P:3,0 bis 3,5     I:0,010 und 0,050       D:5 und 25 
+PID     Controller(P_VALUE, I_VALUE, D_VALUE, RATE);
 
 
 // global varibles
@@ -41,27 +45,17 @@
 unsigned long   time_read_sensors = 0;
 float           angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw]
 float           tempangle = 0; // temporärer winkel für yaw ohne kompass
-float pidtester;
+float co; // PID test
 
 void get_Data() // method which is called by the Ticker Datagetter every RATE seconds
 {
     time_read_sensors = GlobalTimer.read_us();
     
-    // read data from sensors
-    /*GPIO_IntDisable(0, 18, 2); // abschalten der Pins 11-14 mit Göttis library
-    GPIO_IntDisable(0, 17, 2);
-    GPIO_IntDisable(0, 16, 2);
-    GPIO_IntDisable(0, 15, 2);*/
-    //__disable_irq(); // test, deactivate all interrupts, I2C working?
+    // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
     Gyro.read();
-    //Acc.read();
-    //Comp.read();
+    Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig
+    Comp.read();
     //Alt.Update(); TODO braucht zu lange zum auslesen!
-    //__enable_irq();
-    /*GPIO_IntEnable(0, 18, 2); // schaltet die PINs wieder ein
-    GPIO_IntEnable(0, 17, 2);
-    GPIO_IntEnable(0, 16, 2);
-    GPIO_IntEnable(0, 15, 2);*/
     
     dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
     
@@ -75,22 +69,38 @@
     tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
     angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here
     
-    // TODO Read RC data
+    // PID controlling
+    Controller.setProcessValue(angle[1]);
     
     // calculate new motorspeeds
-    /*
-    Motor[0] = 1000 + (100 + (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
-    Motor[1] = 1000 + (100 + (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
-    Motor[2] = 1000 + (100 - (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
-    Motor[3] = 1000 + (100 - (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
-    */
+    co = Controller.compute() - 1000;
+    if (RC[2].read() > 1100) // zu SICHERHEIT! zum testen (ersetzt armed unarmed)
+    {
+        /*Motor[0] = RC[2].read()+((RC[0].read() - 1500)/10.0)+40;
+        Motor[2] = RC[2].read()-((RC[0].read() - 1500)/10.0)-40;*/
+        Motor[0] = RC[2].read()+co+40;
+        Motor[2] = RC[2].read()-co-40;
+    } else {
+        Motor[0] = 1000;
+        Motor[1] = 1000;
+        Motor[2] = 1000;
+        Motor[3] = 1000;
+    }
+    /*Motor[0] = 1000 + (100 - (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000;
+    Motor[1] = 1000 + (100 - (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;
+    Motor[2] = 1000 + (100 + (angle[1] * 500/90)) * (RC[2].read() - 1000) / 1000;
+    Motor[3] = 1000 + (100 + (angle[0] * 500/90)) * (RC[2].read() - 1000) / 1000;*/
 }
 
 int main() { // main programm only used for initialisation and debug output
     NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
-    /*NVIC_SetPriority(I2C0_IRQn, 1); //I2C Priorität?
-    NVIC_SetPriority(I2C1_IRQn, 1);
-    NVIC_SetPriority(I2C2_IRQn, 1);*/
+    
+    //for(int i=0;i<3;i++)
+        Controller.setInputLimits(-90.0, 90.0);
+        Controller.setOutputLimits(0.0, 2000.0);
+        Controller.setBias(1000);
+        Controller.setMode(MANUAL_MODE);//AUTO_MODE);
+        Controller.setSetPoint(0);
     
     #ifdef COMPASSCALIBRATE
         pc.locate(10,5);
@@ -121,7 +131,7 @@
         pc.locate(10,13);
         //pc.printf("Comp_scale: %6.4f %6.4f %6.4f   ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); no more accessible its private
         pc.locate(10,15);
-        pc.printf("pidtester: %6.1f   RC: %d %d %d %d     ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
+        pc.printf("PID Test: %6.1f", co);
         
         pc.locate(10,19);
         pc.printf("RC0: %4d :[", RC[0].read());