File content as of revision 7:f3f94eadc5b5:

#include "mbed.h"
#define _MBED_
#include "Adafruit_9DOF.h"
#include "Serial_base.h"
#include "controller.h"
#include "sensor.h"

#include "quadcopter.h"

DigitalOut myled(LED1);
Adafruit_9DOF dof = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified mag = Adafruit_LSM303_Mag_Unified(30302);
Adafruit_L3GD20_Unified gyro = Adafruit_L3GD20_Unified(20);

Serial pc(USBTX, USBRX);


// pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
PwmOut motor_1(p21);
PwmOut motor_2(p22);
PwmOut motor_3(p23);
PwmOut motor_4(p24);


/*struct pwmOut {
// pwmOut: this struct, PwmOut: class defined by mbed, _pwmOut: class memeber of quadcopter class
// pwm outputs for the 4 motors (motor 1 points into x direction, 2 into y direction, 3 -x direction, -y direction
PwmOut _motr;
PwmOut b
PwmOut c;
PwmOut d;
};
*/

int main()
{
    Quadcopter *myQuadcopter; // initialze Quadcopter object 
    
    control result; 
    offset offset_gyro;

    initSensors(accel, mag, gyro,offset_gyro);

    sensors_event_t accel_event;
    sensors_event_t mag_event;
    sensors_event_t gyro_event;
    sensors_vec_t   orientation;

    motor_1.period(0.002);          // motor requires a 2ms period
    motor_2.period(0.002);          // motor requires a 2ms period
    motor_3.period(0.002);          // motor requires a 2ms period
    motor_4.period(0.002);          // motor requires a 2ms period

// pwm duty cycles for the 4 motors
    motor_1 = 0;
    motor_2 = 0;
    motor_3 = 0;
    motor_4 = 0;

    // startup procedure
    pc.printf("Type 's' to start up Motors, or anything else to abort.\n\r");
    char a= pc.getc();
    if (a!='s') {
        pc.printf("Aborting");
        return 0;
    };
    pc.printf("Starting up ESCs\n\r");
    motor_1 = 0.5;
    motor_2 = 0.5;
    motor_3 = 0.5;
    motor_4 = 0.5;

    pc.printf("Type 'c' to enter control loop, or anything else to abort.\n\r");
    char b= pc.getc();
    if (b!='c') {
        pc.printf("Aborting");
        return 0;
    };

    pc.printf("Entering control loop\n\r");

    while (1) {
        /* Calculate pitch and roll from the raw accelerometer data */
        accel.getEvent(&accel_event);
        if (dof.accelGetOrientation(&accel_event, &orientation)) {
            /* 'orientation' should have valid .roll and .pitch fields */
            //s_com->print(("Roll: "));
            //s_com->print(orientation.roll);
            //s_com->print(("; "));
            //s_com->print(("Pitch: "));
            //s_com->print(orientation.pitch);
            //s_com->print((";\t"));
        }
        /* Calculate the heading using the magnetometer */
        mag.getEvent(&mag_event);
        if (dof.magGetOrientation(SENSOR_AXIS_Z, &mag_event, &orientation)) {
            /* 'orientation' should have valid .heading data now */
            //s_com->print(("Heading: "));
            //s_com->print(orientation.heading);
            //  s_com->print((";\r\n"));
        }
        /* Get angular rate data from gyroscope */
        gyro.getEvent(&gyro_event);
        gyro_event.gyro.x -= offset_gyro.x_offset;
        gyro_event.gyro.y -= offset_gyro.y_offset;
        gyro_event.gyro.z -= offset_gyro.z_offset;

        wait(0.01);

        // get sensor values

        // call controller
        controller(gyro_event, orientation, &result);

        // compute PWM singals
        // assumption for low angles the computed moments are between -10...10
        // since I dont want to risk, i set PWM to 60% duty cycle if deflection =10.
        // USB connection of quadcopter points into x direction.
        
        
        // Set duty cycle
        // continue looking what pwm is. 

        motor_2=0.6+result.M_x/100;
        motor_4=0.6-result.M_x/100;


        // plot

        //pc.printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  F, M_x, M_y, M_z);
        pc.printf("M_x: %f\tM_y: %f\tM_z: %f\tF: %f\n\r", result.M_x, result.M_y, result.M_z, result.F);
    }
}