File content as of revision 8:57c2b7c94ce8:

#include "mbed.h"
#include "rtos.h"
#include "QEI.h"
#include <fstream>
#include <iomanip>
#include "stdlib.h"

#define MOTOR_PPR 300
#define ENCODER_PPR 1024

#define ENC_QUADRATURE_TYPE  4
#define MOT_QUADRATURE_TYPE  2
#define OUR_PI  3.141592653589793
#define DATA_COLS 7
#define BUFFER_SIZE 4200
#define MAX_VOLTAGE 3.3
#define VOLTS_PER_AMP 0.14
#define PROGRAM_RUNTIME 15.0

Serial pc(USBTX, USBRX);

QEI encoder(p29, p30, NC, ENCODER_PPR, QEI::X4_ENCODING);
QEI motor(p25, p26, NC, MOTOR_PPR);
Timer T;

//Curent Measurement
AnalogIn aIn(p16);    //pin 15 set as analog input.  Pins 15-20 can be used as analog inputs.

//Motor direction and PWM
DigitalOut dOut1(p5);
DigitalOut dOut2(p7);
PwmOut pwmOut(p21);

// open a file for data logger
LocalFileSystem local("local");
float theta1, theta2, dtheta1, dtheta2, dtheta2MvFil;
float mCurrent = 0.0;
float inputVoltage = 0.0;
//int pulses0 = 0;
//int deltaPulses;
float t0 = 0.0;
float t = 0.0, dt;

//Controller gains - Full-state Feedback
//float k1 = -0.0316, k2 = 9.7076, k3 = -0.4095, k4 = 1.2340, k5 = 0.0410;
float k1 = -0.3162, k2 = 18.278, k3 = -0.8964, k4 = 2.4441, k5 = 0.1843;

float encoder_conv = 2*OUR_PI/(float(ENCODER_PPR)*float(ENC_QUADRATURE_TYPE));
float motor_conv = 2*OUR_PI/(float(MOTOR_PPR)*float(MOT_QUADRATURE_TYPE));

float* buffer;
float lambda1 = 30, lambda2 = 30, lambda3 = 15;
int index;
int pulsesPend, pulsesMot;
bool flag = 0;

void saving(void const *args) {
    index = 0;
    while ((index < BUFFER_SIZE)&&(flag == 1)&&(false))  {
        buffer[index] = theta1;
        buffer[index+1] = theta2;
        buffer[index+2] = dtheta1;
        buffer[index+3] = dtheta2;
        buffer[index+4] = mCurrent;
        buffer[index+5] = inputVoltage;
        buffer[index+6] = t;
        index = index + DATA_COLS;
        Thread::wait(20);
    }
}

float c2,
g = 9.8,
m2 = 0.0391 + 0.0259,
L1 = (51+44.55)*0.001,
l1 = -0.03478,
L2 = 0.2983,
l2 = 0.05847,
I2x = 0.000534,
I2y = 0.000841,
I2z = 0.00031, 
Ixz2 = -0.00024;
float currentEnergy;

float calcEnergy(void)
{
c2 = cos(theta2);
return (I2x*dtheta2*dtheta2)/2.0 + (I2y*dtheta1*dtheta1)/2.0 + (L1*L1*dtheta1*dtheta1*m2)/2.0 + (dtheta1*dtheta1*l2*l2*m2)/2.0 + (dtheta2*dtheta2*l2*l2*m2)/2.0 
- (I2y*dtheta1*dtheta1*c2*c2)/2.0 + (I2z*dtheta1*dtheta1*c2*c2)/2.0 + Ixz2*dtheta1*dtheta2*c2 + g*l2*m2*c2 - (dtheta1*dtheta1*l2*l2*m2*c2*c2)/2.0 
- L1*dtheta1*dtheta2*l2*m2*c2;
}

void setVoltage(float inputVoltage)
{
    if(inputVoltage<0.0) {
        inputVoltage = -inputVoltage;
        dOut1=0;
        dOut2=1;
    } else {
        dOut1=1;
        dOut2=0;
    }
    float dutyCycle = inputVoltage/MAX_VOLTAGE;
    dutyCycle = (dutyCycle > 1.0)? 1.0 : dutyCycle;
    pwmOut.write(dutyCycle);
}
//
//#define MOVING_AVERAGE_SIZE 10
//
//float movingAverage(float unfiltered)
//{
//    static std::deque<float> movAvgBuffer;              // empty deque of floats
//    static float sum = 0.0;
//    
//    movAvgBuffer.push_front(unfiltered);
//    sum += unfiltered;
//    
//    if( movAvgBuffer.size() <= MOVING_AVERAGE_SIZE)
//    {
//        return (sum/float(movAvgBuffer.size()));
//    }
//    else
//    {
//        sum -= movAvgBuffer.back();
//        movAvgBuffer.pop_back();
//    
//        return (sum/float(MOVING_AVERAGE_SIZE));
//    }
//}

void computing(void const *args) {
    float z1 = 0.0, z2 = 0.0, dz1 = 0.0, dz2 = 0.0, z3 = 0.0, dz3 = 0.0; 
    
    while (true ) {
        t = T.read();
        
       
        //read current
        mCurrent = aIn.read()*MAX_VOLTAGE/VOLTS_PER_AMP; 
        if(dOut1 == 0)
            mCurrent = -mCurrent;
        pulsesPend = -encoder.getPulses();
        pulsesMot = motor.getPulses();
        
        dt = t - t0; //time difference
        theta2 = float(pulsesPend)*encoder_conv + OUR_PI;
        theta1 = float(pulsesMot)*motor_conv;
        
        //calculate dtheta1 
        dz1 = - lambda1 * z1 + lambda1 * theta1;
        z1 = z1 + dz1 * dt;
        dtheta1 = dz1;
        
        //calculate dtheta2 
        dz2 = - lambda2 * z2 + lambda2 * theta2;
        z2 = z2 + dz2 * dt;
        dtheta2 = dz2;
        
        //dtheta2MvFil = movingAverage(dtheta2);
        
        //filter current 
        dz3 = -lambda3 * z3 + lambda3 * mCurrent;
        z3 = z3 + dz3 * dt;
        mCurrent = z3;
        
                //set pwm 
        // ADD A SANITY CHECK ON THETA
        if (cos(theta2) < 0.98) {
            flag = 0;
            currentEnergy = calcEnergy();
            inputVoltage = -2.0*dtheta2*(0.0372 - currentEnergy); 
                    
            
        } else {
            flag = 1;
            inputVoltage = 0.0;
            //inputVoltage = -(k1*theta1 + k2*theta2 + k3*dtheta1 + k4*dtheta2 + k5*mCurrent);
        }
        setVoltage(inputVoltage);
        
        t0 = t;
        Thread::wait(1);
    }
}

void saveToFile ()
{
    FILE *fp = fopen("/local/data.csv", "w");
    if (!fp) {
        fprintf(stderr, "File could not be openend \n\r");
        exit(1);
    }
 
    wait(2.0);
 
    for (int i=0; i < index; i = i + DATA_COLS) 
    {
        for (int j = 0; j < DATA_COLS; j++)
        {
            fprintf(fp,"%f,", buffer[i+j]);
        }
        fprintf(fp,"\n");
    }
    pc.printf("closing file\n\r");
    fclose(fp);
    wait(2.0);;
}

int main() {
    //allocate memory for the buffer
    pc.printf("creating buffer!\r\n");
    buffer = new float[BUFFER_SIZE];
    pc.printf("done creating buffer!\r\n");
    T.start();
    pwmOut.period(0.0001); 
    
    Thread thrd2(computing,NULL,osPriorityRealtime);
    pc.printf("started computing thread!\r\n");
    Thread thrd3(saving,NULL,osPriorityNormal);
    pc.printf("started saving thread!\r\n");
   
    
    pc.printf("Start!\r\n");
    pc.printf("Time: %f\r\n", t);
    
    while (t < PROGRAM_RUNTIME) 
    {
        pc.printf("at time: %f energy: %f\n",t, currentEnergy);
        
        Thread::wait(200);
    }
    pc.printf("Done at Index: %d\r\n",index);
    pwmOut.write(0.0);
    thrd2.terminate();
    thrd3.terminate();
    saveToFile();
}