File content as of revision 4:614517a6e2af:

#include "mbed.h"
#include "PID.h"

//I2C settings
    #define SDA D10
    #define SCL D11
    #define motor_addr 0x91
    #define I2C_BUFFER_SIZE 10
    I2CSlave slave(SDA,SCL);

//Curve settings
    #define CURVE_BUFFER_SIZE 100
    enum curve_t    { OFF=0, CONSTANT_PRESSURE, CONSTANT_FLOW, CONSTANT_SPEED, MODE_SINUS, MODE_ARTERIAL
                    } curve_mode;
    float curve_buffer[CURVE_BUFFER_SIZE];
    float curve_min=80;    //[mmHg]
    float curve_max=120;   //[mmHg]
    float curve_period=10; //[ms]
    int curve_step_us=(int)((curve_period*1000)/CURVE_BUFFER_SIZE);//[us]
//Constant variables
    float constant_pressure;
    float constant_flow;
    float constant_speed;
//Sensor variables
    float sensor_pressure;
    float sensor_flow;
    
//PID settings
        #define PID_PERIOD_US 1000
        #define PID_INITIAL_SETPOINT 0
    //Tuning parameters
        #define Kp 0.1
        #define Ki 0
        #define Kc 0
    //Input is a pressure between 0 and 300 mmHg
        #define PID_MIN_INPUT 0
        #define PID_MAX_INPUT 300
    //Output is a PWM duty cycle
        #define PID_MIN_OUTPUT 0
        #define PID_MAX_OUTPUT 1
    //PID IO
        #define PID_PIN A0
        float ProcessValue = 0;
    //PID Controller
        PID controller(Kp, Ki, Kc, PID_PERIOD_US);
        AnalogOut PID_Output(PID_PIN);

//Note: om de frequentie aan te passen speel je de buffer sneller af. Hierbij neemt nauwkeurigheid wel af. Om dit te verminderen
//heb je meer punten in de buffer nodig.
void curve_sinus(){
    float amplitude = (curve_max - curve_min)/2; //amplitude*sin(t) //van -amplitude naar +amplitude
    //Als sin(x) = 0, moet de curve exact in het midden van max en min zitten
    float offset = (curve_max+curve_min)/2;
    //Genereer een volle periode en zet het in de buffer
    float step = 2*3.1415926/CURVE_BUFFER_SIZE;
    for(int i=0;i<CURVE_BUFFER_SIZE;i++){        
        curve_buffer[i] = offset+amplitude*sin(step*i);
    }
}

/*
float curve_arterial(){
    //Help.
}
*/

int char2_to_int(char* arr, int first_element = 0){
    return (arr[first_element]<<8)+arr[first_element+1];
}

//Returns true if it was succesfull at updating
bool updateVariables(char* arr){
    enum commands_t {   SET_MODE = 1, SET_CONSTANT_PRESSURE, SET_CONSTANT_FLOW, SET_CONSTANT_SPEED, SET_MIN,
                        SET_MAX, SET_FREQUENCY, RECEIVE_PRESSURE, RECEIVE_FLOW
                    };
    commands_t command_codes = (commands_t)arr[0];
    switch(command_codes){
        case SET_MODE:
            curve_mode = (curve_t)char2_to_int(arr,1);
            break;
        case SET_CONSTANT_PRESSURE:
            constant_pressure = char2_to_int(arr,1)/100.0;
            break;
        case SET_CONSTANT_FLOW:
            constant_flow = char2_to_int(arr,1)/10.0;
            break;
        case SET_CONSTANT_SPEED:
            constant_speed = (float)char2_to_int(arr,1);
            break;
        case SET_MIN:
            curve_min = char2_to_int(arr,1)/100.0;
            break;
        case SET_MAX:
            curve_max = char2_to_int(arr,1)/100.0;
            break;
        case SET_FREQUENCY:
            //Note: it receives a frequency but internally converts it to
            //      a period in ms immediately.
            curve_period = 1000/((char2_to_int(arr,1))/100.0);
            break;
        case RECEIVE_PRESSURE:
            sensor_pressure = char2_to_int(arr,1)/100.0;
            break;
        case RECEIVE_FLOW:
            sensor_flow = char2_to_int(arr,1)/10.0;
            break;  
        default:
            //No command was valid
            //Create some kind of error? Maybe, a blinking led.
            return false;  
    } 
    return true;
}

void PID_loop(Timer& t_PID, Timer& t_Curve){
    static int PID_last_curve_point = 0;
    //Check if the PID scheme has to execute again
    if(t_PID.read_us() >= PID_PERIOD_US){
        bool PID_on = true;
        //What mode is the motorcontroller currently in?
        switch(curve_mode){
            case CONSTANT_PRESSURE:
                //Set Point is the value we want to achieve
                //Which in this case is constant
                //Process Value is the current value of the process
                //Which is variable and connected to processes in the physical world
                controller.setSetPoint(constant_pressure);
                controller.setProcessValue(sensor_pressure);
                break;
            case CONSTANT_FLOW:
                controller.setSetPoint(constant_flow);
                controller.setProcessValue(sensor_flow);
                break;
            case CONSTANT_SPEED:
                //There is already a PI control scheme on the Maxon module
                //We only have to send a PWM signal and the module will do the rest
                //TODO: Clip this back to a value between 0 and 1
                PID_Output = constant_speed;
                PID_on = false;
                break;
            case MODE_SINUS:
                //Take the current point in the buffer
                controller.setSetPoint(curve_buffer[PID_last_curve_point]);
                controller.setProcessValue(sensor_pressure);
                break;
            case MODE_ARTERIAL:
                controller.setSetPoint(curve_buffer[PID_last_curve_point]);
                controller.setProcessValue(sensor_pressure);
                break;
            default:
                //Curve should be off or the mode is invalid
                PID_on = false;
                break;  
        }
        if(PID_on == true){
            //Adjust output according to the PID scheme
            PID_Output = controller.compute();
        }
        //For the next iteration, check whether it'll have to be a new point on the curve
        //Note: putting this at the end, ensures that in the first loop point 0 will get executed
        if(t_Curve.read_us() >= curve_step_us){
            PID_last_curve_point++;
            if(PID_last_curve_point >= CURVE_BUFFER_SIZE){
                PID_last_curve_point = 0;    
            }
            //Reset the curve timer
            t_Curve.reset();    
        }
        //Reset the PID timer
        t_PID.reset();
    } 

    
}

int main() {
    slave.address(motor_addr); //Set the correct address for this module
        
    //PID Controller initialization
    controller.setInputLimits(PID_MIN_INPUT, PID_MAX_INPUT);
    controller.setOutputLimits(PID_MIN_OUTPUT, PID_MAX_OUTPUT);
    controller.setSetPoint(PID_INITIAL_SETPOINT);
        
    Timer t_PID;
    Timer t_Curve;    
    t_PID.start();
    t_Curve.start();
    
    char buffer[I2C_BUFFER_SIZE] = {0}; //Create the buffer for I2C
    bool buffer_changed = false;
    
    while(1) {
        int i = slave.receive();        
        switch (i) {
            case I2CSlave::ReadAddressed:
                //Received a request to be read
                //Irrelevant for now
                break;
            case I2CSlave::WriteGeneral:
                //Received a request to be written to
                slave.read(buffer,I2C_BUFFER_SIZE);
                buffer_changed = true;
                break;
            case I2CSlave::WriteAddressed:
                //Received a request to be written to a specific location
                slave.read(buffer,I2C_BUFFER_SIZE);
                buffer_changed = true;
                break;
        }
        //Do shit depending on the command it received in the first data block
        if(buffer_changed == true){
            updateVariables(buffer);
            //Clear the buffer for the next iteration
            for(int i=0;i<I2C_BUFFER_SIZE;i++){
                buffer[i] = 0;    
            }
            buffer_changed = false;
        }
        //Execute the PID loop
        PID_loop(t_PID,t_Curve);
        
    }
}