File content as of revision 1:17fdd812cb8d:

#include "mbed.h"
#include "EncoderCounter.h"
#include "Controller.h"
#include "IMU.h"

DigitalOut myled(LED1);
DigitalOut enable(PB_15);

// create IMU comunication objects
SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
DigitalOut csAG(PA_15);
DigitalOut csM(PD_2);

//create IMU object
IMU imu(spi, csAG, csM);

// initialise PWM
PwmOut pwm_motor1(PB_13);
PwmOut pwm_motor2(PA_9);

// crete Encoder read objects
EncoderCounter counter1(PA_6, PC_7); // counter(pin A, pin B)
EncoderCounter counter2(PB_6, PB_7);

// create controller object
Controller controller(pwm_motor1, pwm_motor2, counter1, counter2);

DigitalIn user_button(USER_BUTTON);

int main()
{
    // initialise controller for 2 DC-Motor
    controller.setDesiredSpeedLeft(0.0f); // speed in [rpm]
    controller.setDesiredSpeedRight(0.0f);
    enable = 1;
    
    // variables for measurement in one axis
    int nData = 5000;
    int nSave = 1;
    int delayMeasure = 3000;
    int   * T = new int[nData]();
    float * Mes1 = new float[nData]();
    float * Mes2 = new float[nData]();
    int mesInd;
    
    int t = 0;
    bool sendMeasure = false;
    
    bool stable = true;
    bool previousStable = true;
        
    printf("initialization succesfull\n\r");

    while (1) {

        if(!user_button) {            
            // LED off, set target speed at 100rpm
            myled = 0;
            controller.setDesiredSpeedLeft(60.0f); // set speed 100rpm
            controller.setDesiredSpeedRight(60.0f);    
        } else {
            // LED on, set target speed at 0rpm   
            myled = 1;
            controller.setDesiredSpeedLeft(0.0f); // Drehzahl in [rpm]
            controller.setDesiredSpeedRight(0.0f);
        }
        // display encoder values to console
        printf("speed left = %.2f rpm, speed right = %.2f rpm\r\n",controller.getSpeedLeft(),controller.getSpeedRight());


        float accX = imu.readAccelerationX();

        // save data in object measure-variables
        if (t % nSave == 0 && stable) {
                *(T+mesInd) = t;
                *(Mes1+mesInd) = accX;
                *(Mes2+mesInd) = accX; 
                mesInd ++;
            }
        // reset index if array are full
        if(mesInd == nData){mesInd = 0;}

        // stability condition
        //if(...){stable = false;}
        
        // send measure to console
        if(sendMeasure) {
            printf("SendMeasure\r\n\n");
            for(int j=mesInd; j<(nData-mesInd); j++) {
                printf("%d %.7f %.7f\r\n",*(T+j)-*(T+mesInd),*(Mes1+j),*(Mes2+j));
            }
            for(int j=0; j<mesInd; j++) {
                printf("%d %.7f %.7f\r\n",*(T+j)-*(T+mesInd),*(Mes1+j),*(Mes2+j));
            }
            printf("endData\r\n\n");
            sendMeasure = false;
        }
        
        t++;
        
        wait(0.01f); // Takt 0.01s, 100Hz  
    }
}