File content as of revision 1:e5d4451e1144:

    #include "mbed.h"
#include "USBHID.h"
#include "ArduinoMotorShield.h"
#include "motorControl.h"
#include "PID.h"
#include "Motor_shield.h"
#include "HIH_6130.h"
#include "globalt.h"


//We declare a USBHID device. By default input and output reports are 64 bytes long.
USBHID hid(63,63,0x03cc,0x1234,0x0001,true);

//We declare a I2C humidity sensor of the type HIH-6130-021
hih6130 humiSensor(PTC11,PTC10);


//This report will contain data to be sent
HID_REPORT send_report;
HID_REPORT send_report_humi;
HID_REPORT recv_report;

//Ticker for having the correct sample rate
Ticker sampleTick;

//Ticker for controlling PID controller
Ticker controlTick;


//Declare an analog input for pressure1sensors
AnalogIn pressureSensor1(PTB2);
AnalogIn pressureSensor2(PTB3);
AnalogIn pressureSensor3(PTC2);
//Declare an analog inputs for current sense on motor shield
AnalogIn sense01(PTB0);
AnalogIn sense02(PTB1);

//Stepper fot controlling step motor on pump
int stepsPerRevolution = 200;  // change this to fit the number of steps per revolution

//RGB LED defintion
PwmOut red (LED_RED);
PwmOut green (LED_GREEN);
PwmOut blue (LED_BLUE);





// initialize the motor shield library (DC motor)
Motor_shield m(PTA12, PTD3, PTA13); // pwm, fwd, brake

// for your motor
//Initialize the stepper library:
#ifdef A4980PWM
motorControl pump(stepsPerRevolution, PTC9, PTC7, PTC3, PTC6, PTC13, PTC5, PTC4); // creates new stepper motor: int numberOfSteps, PinName step, PinName dir, PinName ms1, PinName ms0, PinName enable, PinName reset, PinName diag
#else
ArduinoMotorShield pump(stepsPerRevolution, PTA13, PTD5, PTD3, PTD1, PTA12, PTD2);
#endif





//Global for the wanted pump speed
int pumpSpeed;
int minPressure;
int maxPressure;
int stateTympSweep = 0;
int microSteps = 0;
int tPauseSec = 0;
int tTimeOutSec = 0;

//Global holding the humidity and temperature
int humiTempHolder;

//gobal for handling one sample for the pressure1sensor
unsigned short pressure1= 0;
int pressure2= 0;
int pressure3= 0;

float pressureFloat = 0;
float pumpSpeedFloat = 0;
float setpoint;
float minPressureFloat;
float maxPressureFloat;
float pumpSpeedSetPoint;
float setPointFloat;
bool startSweep = false;
bool startTymp = false;
bool startWear = false;

bool dcMotor = false;
int reportCounter = 0;

int current01;
int current02;




#define RATE 0.1

//Kc, Ti, Td, interval
PID controller(5.0, 5.0, 0, RATE);

//Debug serial port
Serial pc(USBTX, USBRX);

/*     Tick handler for sampling of pressure1sensor            */
void samplepressureSensor1()
{
    pressure1 = pressureSensor1.read_u16();
    current01 = sense01.read_u16();
    current02 = sense02.read_u16();

    pressure2 = pressureSensor2.read_u16();
    pressure3 = pressureSensor3.read_u16();
    //pc.printf("Pres 3: %i ", pressure3);
    //pc.printf("\r\n");
    //Fill the report with sensor data
    if(reportCounter == 0)
    {
        send_report.data[reportCounter] = 'P';
    }
    send_report.data[reportCounter + 1] = 0;
    send_report.data[reportCounter + 2] = pressure1 >> 8;
    send_report.data[reportCounter + 3] = pressure1 & 0xff;
    send_report.data[reportCounter + 4] = 0;
    send_report.data[reportCounter + 5] = current01 >> 8;
    send_report.data[reportCounter + 6] = current01 & 0xff;
    send_report.data[reportCounter + 7] = 0;
    send_report.data[reportCounter + 8] = current02 >> 8;
    send_report.data[reportCounter + 9] = current02 & 0xff;
    send_report.data[reportCounter + 10] = 0;
    send_report.data[reportCounter + 11] = pressure2 >> 8;
    send_report.data[reportCounter + 12] = pressure2 & 0xff;
    send_report.data[reportCounter + 13] = 0;
    send_report.data[reportCounter + 14] = pressure3 >> 8;
    send_report.data[reportCounter + 15] = pressure3 & 0xff;
    //send_report.data[reportCounter + 16] = 0;
    reportCounter += 15; //Counting how many of the 63 bytes is used
    send_report.length = 63;
    if(reportCounter >= 60) {
        //Send the report
        hid.sendNB(&send_report);
        reportCounter = 0;
    }

}

/*     Tick handler computing of HID controller            */
void computeHIDController()
{
    pressureFloat = pressureSensor1.read();


    //Update the process variable.
    controller.setProcessValue(pressureFloat);
    //Set the new output.
    pumpSpeedFloat = controller.compute();


    if(dcMotor) {

        m.speed(pumpSpeedFloat);
        //pc.printf("2 DC speed: %f ", pumpSpeedFloat);
        //pc.printf("\r\n");
    } else {
        if(pumpSpeedFloat < 0) {
            //if(pumpSpeedFloat > -15) {
            //  pumpSpeedFloat = -15;
            //}
            //#ifdef A4980
            //pump.runStepperAtSpeedWithSPI(true,(pumpSpeedFloat*(-1)), 0);
            #ifdef A4980PWM
            pump.runStepperAtSpeed(true,(pumpSpeedFloat*(-1)), 0);
            #else
            pump.runStepperAtSpeed(true,(pumpSpeedFloat*(-1)), 0);
            #endif
            
        } else {
            //if(pumpSpeedFloat < 15) {
            //  pumpSpeedFloat = 15;
            //}
            //#ifdef A4980
            //pump.runStepperAtSpeedWithSPI(true,pumpSpeedFloat, 1);
            #ifdef A4980PWM
            pump.runStepperAtSpeed(true,pumpSpeedFloat, 1);
            #else
            pump.runStepperAtSpeed(true,pumpSpeedFloat, 1);
            #endif
        }
        //pc.printf("Stepper speed: %f : %f ", pumpSpeedFloat,pressureFloat);
        //pc.printf("\r\n");
    }




}

float convertDaPAToNominalFloat(int daPa)
{
    //pc.printf("daPA: %d ", daPa);
    float kPa = ((float) daPa)/100;
    //pc.printf("kPA: %f ", kPa);
    float returnValue;
    returnValue = (0.057*kPa)+0.5;
    return returnValue;
}

/*    Sample analog input and send on HID            */
void samplePressureSensors(int averageNum)
{
    int averPressure1 = 0;
    int averCurrent01 = 0;
    int averCurrent02 = 0;

    int averPressure2 = 0;
    int averPressure3 = 0;
    
        
    int i = 0;
    for(i = 0; i < averageNum; i++)
    {
        averPressure1 += pressureSensor1.read_u16();
        averCurrent01 += sense01.read_u16();
        averCurrent02 += sense02.read_u16();

        averPressure2 += pressureSensor2.read_u16();
        averPressure3 += pressureSensor3.read_u16();
        wait(0.001);
    }
    
    
    averPressure1 = averPressure1/averageNum;
    averCurrent01 = averCurrent01/averageNum;
    averCurrent02 = averCurrent02/averageNum;

    averPressure2 = averPressure2/averageNum;
    averPressure3 = averPressure3/averageNum;
    
       
    //Fill the report with sensor data
    send_report.data[0] = 'S';
    send_report.data[1] = 0;
    send_report.data[2] = averPressure1 >> 8;
    send_report.data[3] = averPressure1 & 0xff;
    send_report.data[4] = 0;
    send_report.data[5] = averCurrent01 >> 8;
    send_report.data[6] = averCurrent01 & 0xff;
    send_report.data[7] = 0;
    send_report.data[8] = averCurrent02 >> 8;
    send_report.data[9] = averCurrent02 & 0xff;
    send_report.data[10] = 0;
    send_report.data[11] = averPressure2 >> 8;
    send_report.data[12] = averPressure2 & 0xff;
    send_report.data[13] = 0;
    send_report.data[14] = averPressure3 >> 8;
    send_report.data[15] = averPressure3 & 0xff;
    send_report.data[16] = 0;
    send_report.length = 63;
    hid.sendNB(&send_report);
    
   

}

void stopAll()
{
    sampleTick.detach();
    controlTick.detach();
    pump.openCloseValve(true);
    //#ifdef A4980
    //pump.runStepperAtSpeedWithSPI(false,0,0);
    #ifdef A4980PWM
    pump.runStepperAtSpeed(false,0,0);
    #else
    pump.runStepperAtSpeed(false,0,0);
    #endif
    m.speed(0);
    startSweep = false;
    startTymp = false;
    startWear = false;
    reportCounter = 0;
    pressureFloat = 0.5;
    pc.printf("Stop");
    pc.printf("\r\n");
}

void stopMotor()
{
    //sampleTick.detach();
    controlTick.detach();
    //#ifdef A4980
    //pump.runStepperAtSpeedWithSPI(false,0,0);
    #ifdef A4980PWM
    pump.runStepperAtSpeed(false,0,0);
    #else
    pump.runStepperAtSpeed(false,0,0);
    #endif
    m.speed(0);
    
}

void sendWearFinished()
{
    //Fill the report with sensor data
    send_report.data[0] = 'W';
    send_report.data[1] = 0;
    send_report.data[2] = 'F';
    send_report.data[1] = 0; 
    send_report.length = 63;
    hid.sendNB(&send_report);
}

void sendWearLeakFinished()
{
    //Fill the report with sensor data
    send_report.data[0] = 'W';
    send_report.data[1] = 0;
    send_report.data[2] = 'L';
    send_report.data[1] = 0; 
    send_report.length = 63;
    hid.sendNB(&send_report);
}

//Parser for HID reports
void parseIncommingMessage( )
{
    int setPointMessage;
    float setPointMessageFloat;
    //-------------------------------------------------------STOP-----------------------------------------------------------------------------------// 
    if(recv_report.data[0] == 'S' && recv_report.data[1] == 'T' && recv_report.data[2] == 'O' && recv_report.data[3] == 'P') {
       stopAll(); 
    }
    //-------------------------------------------------------STOP MOTOR-----------------------------------------------------------------------------------// 
    if(recv_report.data[0] == 'S' && recv_report.data[1] == 'T' && recv_report.data[2] == 'M' && recv_report.data[3] == 'O') {
       stopMotor(); 
    }
    //-------------------------------------------------------SET OUTPUT PIN-----------------------------------------------------------------------------------// 
    if(recv_report.data[0] == 'O' && recv_report.data[1] == 'U' && recv_report.data[2] == 'T' && recv_report.data[3] == 'P') {
        pc.printf("Set Output");
        pc.printf("\r\n");
        if(recv_report.data[5] == 'F')
        {
            pump.openCloseValve(false);
            pc.printf("True");
            pc.printf("\r\n");
        }
        else
        {
            pump.openCloseValve(true);
            pc.printf("False");
            pc.printf("\r\n");
        }
       
    }
    //-------------------------------------------------------SWEEP-----------------------------------------------------------------------------------// 
    else if(recv_report.data[0] == 'S' && recv_report.data[1] == 'W' && recv_report.data[2] == 'E' && recv_report.data[3] == 'E' && recv_report.data[4] == 'P') {
        pumpSpeed = (((int)(recv_report.data[6]) - 0x30) * 100) + (((int)(recv_report.data[7]) - 0x30) * 10) + (((int)(recv_report.data[8]) - 0x30));
        minPressure = -1*(((recv_report.data[10] - 0x30) * 100) + ((recv_report.data[11] - 0x30) * 10) + ((recv_report.data[12] - 0x30)));
        maxPressure = ((recv_report.data[14] - 0x30) * 100) + ((recv_report.data[15] - 0x30) * 10) + ((recv_report.data[16] - 0x30));
        minPressureFloat = convertDaPAToNominalFloat(minPressure);
        maxPressureFloat = convertDaPAToNominalFloat(maxPressure);
        
        //True if DC motor
        if(recv_report.data[18] == 'T' && recv_report.data[19] == 'r' && recv_report.data[20] == 'u' && recv_report.data[21] == 'e') {
            dcMotor = true;
            //float pumpspeedNorm = ((( (float) pumpSpeed-15)/6.75)/24); /// HACK for testing pump from WPM
            controller.setBias(0.3333);
            controller.setOutputLimits(-1, 1);
            microSteps = (recv_report.data[23] - 0x30); //MicroSteps
            // pc.printf("DC Motor: %d",pumpspeedNorm);
            //pc.printf("\r\n");
        } else {
            dcMotor = false;
            //RPM output (stepper motor) from -400.0 to 400.0
            controller.setBias(2.0);
            controller.setOutputLimits(-pumpSpeed, pumpSpeed);
            microSteps = (recv_report.data[24] - 0x30); //MicroSteps
            //pc.printf("Stepper Motor");
            //pc.printf("\r\n");

        }
        
        
        pc.printf("MicroSteps: %i", microSteps);
        pc.printf("\r\n");
        
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        pc.printf("Get MicroSteps: %i", pump.getMicroSteps());
        pc.printf("\r\n");
        #endif
        
        pump.openCloseValve(false);
        controller.setSetPoint(minPressureFloat);
        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
        sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds)
        startSweep = true;
        startTymp = false;
        startWear = false;
    }
    //-------------------------------------------------------Const Presssure-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'P' && recv_report.data[1] == 'R' && recv_report.data[2] == 'E' && recv_report.data[3] == 'S' ) {
        startSweep = false;
        pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));

        //check if negative pressure1is wanted
        int negativevalue = 0;
        if(recv_report.data[9] == '-') {
            negativevalue = 1;
            setPointMessage = -1*(((int)(recv_report.data[9 + negativevalue]) - 0x30) * 100) + (((int)(recv_report.data[10 + negativevalue]) - 0x30) * 10) + (((int)(recv_report.data[11 + negativevalue]) - 0x30));
        } else {
            setPointMessage = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30));
        }


        setPointMessageFloat = convertDaPAToNominalFloat(setPointMessage);

        //True if DC motor
        if(recv_report.data[13 + negativevalue] == 'T' && recv_report.data[14 + negativevalue] == 'r' && recv_report.data[15 + negativevalue] == 'u' && recv_report.data[16 + negativevalue] == 'e') {
            dcMotor = true;
            controller.setOutputLimits(-1.0, 1.0); //full output limits
            controller.setBias(0.0);
            microSteps = (recv_report.data[18] - 0x30); //MicroSteps
            // pc.printf("DC Motor");
            // pc.printf("\r\n");
        } else {
            dcMotor = false;
            controller.setBias(2.0);
            //RPM output (stepper motor) from -400.0 to 400.0
            controller.setOutputLimits(-pumpSpeed, pumpSpeed);
            microSteps = (recv_report.data[19] - 0x30); //MicroSteps
            // pc.printf("Stepper Motor");
            // pc.printf("\r\n");
        }
        
        pc.printf("MicroSteps: %i", microSteps);
        pc.printf("\r\n");
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        #endif
        pc.printf("CONST PRES SETPOINT: %i %f ",setPointMessage,setPointMessageFloat);
        pc.printf("\r\n");
        pump.openCloseValve(false);
        controller.setSetPoint(setPointMessageFloat);
        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
        sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds)


    }
    //-------------------------------------------------------LEAK-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'L' && recv_report.data[1] == 'E' && recv_report.data[2] == 'A' && recv_report.data[3] == 'K' ) {
        startSweep = false;
        pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));

        //check if negative pressure1is wanted
        int negativevalue = 0;
        if(recv_report.data[9] == '-') {
            negativevalue = 1;
            setPointMessage = -1*(((int)(recv_report.data[9 + negativevalue]) - 0x30) * 100) + (((int)(recv_report.data[10 + negativevalue]) - 0x30) * 10) + (((int)(recv_report.data[11 + negativevalue]) - 0x30));
        } else {
            setPointMessage = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30));
        }


        setPointMessageFloat = convertDaPAToNominalFloat(setPointMessage);

        //True if DC motor
        if(recv_report.data[13 + negativevalue] == 'T' && recv_report.data[14 + negativevalue] == 'r' && recv_report.data[15 + negativevalue] == 'u' && recv_report.data[16 + negativevalue] == 'e') {
            dcMotor = true;
            controller.setOutputLimits(-1.0, 1.0); //full output limits
            controller.setBias(0.0);
            // pc.printf("DC Motor");
            // pc.printf("\r\n");
        } else {
            dcMotor = false;
            controller.setBias(2.0);
            //RPM output (stepper motor) from -400.0 to 400.0
            controller.setOutputLimits(-pumpSpeed, pumpSpeed);
            // pc.printf("Stepper Motor");
            // pc.printf("\r\n");
        }
         pc.printf("LEAK CONST PRES SETPOINT: %i %f ",setPointMessage,setPointMessageFloat);
        pc.printf("\r\n");
        pump.openCloseValve(false);
        controller.setSetPoint(setPointMessageFloat);
        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
        sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds)
        stateTympSweep = 4;
        setPointFloat = setPointMessageFloat;
        startTymp = true;
        startSweep = false;
        startWear = false;
        pc.printf("LEAK start");


    }
    //-------------------------------------------------------Humidity-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'H' && recv_report.data[1] == 'U' && recv_report.data[2] == 'M' && recv_report.data[3] == 'I' )
    {
        pc.printf("HUMI");
        pc.printf("\r\n");
        int data = humiSensor.fetchDataRaw();
        send_report.data[0] = 'H';
        send_report.data[1] = 0;
        send_report.data[2] = (data & 0xff000000) >> 24;
        send_report.data[3] = (data & 0xff0000) >> 16;
        send_report.data[4] = (data & 0xff00) >> 8;
        send_report.data[5] = (data & 0xff);
        send_report.data[6] = 0;
        send_report.length = 63;
        
        //Send the report
        hid.send(&send_report);
    }
    //-------------------------------------------------------Sensor measurement-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'S' && recv_report.data[1] == 'E' && recv_report.data[2] == 'N' && recv_report.data[3] == 'S' )
    {
        //pc.printf("SENS");
        //pc.printf("\r\n");
        int averageNum = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));
        //pc.printf("AverageNum %i",averageNum);
        //pc.printf("\r\n");
        samplePressureSensors(averageNum);
    }
    //-------------------------------------------------------RMP-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'R' && recv_report.data[1] == 'P' && recv_report.data[2] == 'M' && recv_report.data[3] == 'S' )
    {
        pc.printf("RPMS");
        pc.printf("\r\n");
        startSweep = false;
        //check if negative pressure1is wanted
        int negativevalue = 0;
        if(recv_report.data[5] == '-') 
        {
            negativevalue = 1;
            pumpSpeed = (((int)(recv_report.data[negativevalue + 5]) - 0x30) * 100) + (((int)(recv_report.data[negativevalue + 6]) - 0x30) * 10) + (((int)(recv_report.data[negativevalue + 7]) - 0x30));
            pumpSpeed = -1*pumpSpeed;
            microSteps = (recv_report.data[10] - 0x30); //MicroSteps
        }
        else
        {
            pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));
            microSteps = (recv_report.data[9] - 0x30); //MicroSteps
        }
        pc.printf("RPMS pumps %i", pumpSpeed);
        pc.printf("\r\n");
        pump.openCloseValve(false);
        pc.printf("MicroSteps: %i", microSteps);
        pc.printf("\r\n");
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        #endif
        if(pumpSpeed < 0) {
            //#ifdef A4980
            //pump.runStepperAtSpeedWithSPI(true,(pumpSpeed*(-1)), 0);
            #ifdef A4890PWM
            pump.doRamp(10,pump.calculateStepDelay(pumpSpeed*(-1)));
            pump.runStepperAtSpeed(true,(pumpSpeed*(-1)), 0);
            #else
            pump.runStepperAtSpeed(true,(pumpSpeed*(-1)), 0);
            #endif
        } else {
            //#ifdef A4980
            //pump.runStepperAtSpeedWithSPI(true,pumpSpeed, 1);
            #ifdef A4980PWM
            pump.doRamp(10,pump.calculateStepDelay(pumpSpeed));
            pump.runStepperAtSpeed(true,pumpSpeed, 1);
            #else
            pump.runStepperAtSpeed(true,pumpSpeed, 1);
            #endif
        }
    }
    //-------------------------------------------------------STEP-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'S' && recv_report.data[1] == 'T' && recv_report.data[2] == 'E' && recv_report.data[3] == 'P' )
    {
        pc.printf("STEP");
        pc.printf("\r\n");
        startSweep = false;
        //check if negative pressure1is wanted
        int negativevalue = 0;
        int numOfSteps = 0;
        int delayBetweenSteps = 0;
        bool direction = true;
        if(recv_report.data[5] == '-') 
        {
            negativevalue = 1;
            numOfSteps = (((int)(recv_report.data[negativevalue + 5]) - 0x30) * 100) + (((int)(recv_report.data[negativevalue + 6]) - 0x30) * 10) + (((int)(recv_report.data[negativevalue + 7]) - 0x30));
            direction = false;
            delayBetweenSteps = (((int)(recv_report.data[negativevalue + 9]) - 0x30) * 100) + (((int)(recv_report.data[negativevalue + 10]) - 0x30) * 10) + (((int)(recv_report.data[negativevalue + 11]) - 0x30));
            microSteps = (recv_report.data[14] - 0x30); //MicroSteps
        }
        else
        {
            numOfSteps = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));
            delayBetweenSteps = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30));
            microSteps = (recv_report.data[13] - 0x30); //MicroSteps
        }
        pc.printf("STEP numOfSteps %i delay %i microsteps %i", numOfSteps,delayBetweenSteps,microSteps);
        pc.printf("\r\n");
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        #endif
        if(direction) {
           
            //pump.doRamp(10,pump.calculateStepDelay(pumpSpeed*(-1)));
            pump.step(numOfSteps,delayBetweenSteps,true);
            
           
        } else {
            pump.step(numOfSteps,delayBetweenSteps,false);
        }
    }
    
    //-------------------------------------------------------ZERO Posion-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'Z' && recv_report.data[1] == 'E' && recv_report.data[2] == 'R' && recv_report.data[3] == 'O' )
    {
        pc.printf("ZERO");
        pc.printf("\r\n");
        startSweep = false;
        startTymp = false;
        stopAll();
        if(pump.getStepCount() > 0)
        {
            pumpSpeed = -MAX_SPEED;
        }
        else
        {
            pumpSpeed = MAX_SPEED;
        }
        
        //#ifdef A4980
          //pump.goToZeroPosition(pumpSpeed);
        #ifdef A4980PWM
          //pump.goToZeroPosition(pumpSpeed);
          //pump.step(1,1,1);
        #endif
        
    }
    //-------------------------------------------------------TYMP-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'T' && recv_report.data[1] == 'Y' && recv_report.data[2] == 'M' && recv_report.data[3] == 'P') {
        pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));
        minPressure = -1*(((recv_report.data[9] - 0x30) * 100) + ((recv_report.data[10] - 0x30) * 10) + ((recv_report.data[11] - 0x30)));
        maxPressure = ((recv_report.data[13] - 0x30) * 100) + ((recv_report.data[14] - 0x30) * 10) + ((recv_report.data[15] - 0x30));
        minPressureFloat = convertDaPAToNominalFloat(minPressure);
        maxPressureFloat = convertDaPAToNominalFloat(maxPressure);
        
        //True if DC motor
        if(recv_report.data[17] == 'T' && recv_report.data[18] == 'r' && recv_report.data[19] == 'u' && recv_report.data[20] == 'e') {
            dcMotor = true;
            //float pumpspeedNorm = ((( (float) pumpSpeed-15)/6.75)/24); /// HACK for testing pump from WPM
            controller.setBias(0.3333);
            controller.setOutputLimits(-1, 1);
            microSteps = (recv_report.data[22] - 0x30); //MicroSteps
            // pc.printf("DC Motor: %d",pumpspeedNorm);
            //pc.printf("\r\n");
        } else {
            dcMotor = false;
            //RPM output (stepper motor) from -400.0 to 400.0
            controller.setBias(2.0);
            controller.setOutputLimits(-pumpSpeed, pumpSpeed);
            microSteps = (recv_report.data[23] - 0x30); //MicroSteps
            pc.printf("Stepper Motor, speed: %i minPressure: %i maxPressure %i minPressureFloat: %f maxPressureFloat %f", pumpSpeed, minPressure, maxPressure, minPressureFloat, maxPressureFloat);
            pc.printf("\r\n");

        }
        
        
        pc.printf("MicroSteps: %i", microSteps);
        pc.printf("\r\n");
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        #endif
        
        pump.openCloseValve(false);
        controller.setSetPoint(minPressureFloat);
        stateTympSweep = 1;
        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
        sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds)
        startTymp = true;
        startSweep = false;
        startWear = false;
    } 
    
    //-------------------------------------------------------WEAR Leak test-----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'W' && recv_report.data[1] == 'L' && recv_report.data[2] == 'E' && recv_report.data[3] == 'A') {
        pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));

        //check if negative pressure1is wanted
        int negativevalue = 0;
        if(recv_report.data[9] == '-') {
            negativevalue = 1;
            setPointMessage = -1*(((int)(recv_report.data[9 + negativevalue]) - 0x30) * 100) + (((int)(recv_report.data[10 + negativevalue]) - 0x30) * 10) + (((int)(recv_report.data[11 + negativevalue]) - 0x30));
        } else {
            setPointMessage = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30));
        }
        

        setPointFloat = convertDaPAToNominalFloat(setPointMessage);
        
       dcMotor = false;
       controller.setBias(2.0);
       controller.setOutputLimits(-pumpSpeed, pumpSpeed);
       microSteps = (recv_report.data[13 + negativevalue] - 0x30); //MicroSteps
       pc.printf("Wear leak test, speed: %i setPointMessage: %i ", pumpSpeed, setPointMessage);
       pc.printf("\r\n");

        
        
        
        pc.printf("MicroSteps: %i", microSteps);
        pc.printf("\r\n");
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        #endif
        
        pump.openCloseValve(false);
        controller.setSetPoint(setPointFloat);
        stateTympSweep = 4;
        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
        //sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds)
        startWear = true;
        startTymp = false;
        startSweep = false;
    } 
    //-------------------------------------------------------WEAR -----------------------------------------------------------------------------------//
    else if(recv_report.data[0] == 'W' && recv_report.data[1] == 'E' && recv_report.data[2] == 'A' && recv_report.data[3] == 'R') {
        pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30));
        minPressure = -1*(((recv_report.data[9] - 0x30) * 100) + ((recv_report.data[10] - 0x30) * 10) + ((recv_report.data[11] - 0x30)));
        maxPressure = ((recv_report.data[13] - 0x30) * 100) + ((recv_report.data[14] - 0x30) * 10) + ((recv_report.data[15] - 0x30));
        tPauseSec = ((recv_report.data[17] - 0x30) * 100) + ((recv_report.data[18] - 0x30) * 10) + ((recv_report.data[19] - 0x30));
        tTimeOutSec = ((recv_report.data[21] - 0x30) * 100) + ((recv_report.data[22] - 0x30) * 10) + ((recv_report.data[23] - 0x30));
        
        minPressureFloat = convertDaPAToNominalFloat(minPressure);
        maxPressureFloat = convertDaPAToNominalFloat(maxPressure);
        
       dcMotor = false;
       controller.setBias(2.0);
       controller.setOutputLimits(-pumpSpeed, pumpSpeed);
       microSteps = (recv_report.data[25] - 0x30); //MicroSteps
       pc.printf("Wear, speed: %i minPressure: %i maxPressure %i tPause: %i tTimeOut %i", pumpSpeed, minPressure, maxPressure, tPauseSec, tTimeOutSec);
       pc.printf("\r\n");

        
        
        
        pc.printf("MicroSteps: %i", microSteps);
        pc.printf("\r\n");
        #ifdef A4980PWM
        pump.setMicroSteps(microSteps);
        #endif
        
        pump.openCloseValve(false);
        controller.setSetPoint(minPressureFloat);
        stateTympSweep = 1;
        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
        //sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds)
        startWear = true;
        startTymp = false;
        startSweep = false;
    }

}

/* Main loop */

int main()
{

    //Analog input from 0.0 to 3.3V
    controller.setInputLimits(0.0, 1.0); //Todo: should this be 0.0,1.0?
    //RPM output (stepper motor) from -300.0 to 300.0
    controller.setOutputLimits(-300.0, 300.0);
    //If there's a bias.
    controller.setBias(15.0);
    controller.setMode(AUTO_MODE);
    //We want the process variable to be 2.0V
    controller.setSetPoint(0.6);

    //Setup debug port to 115200 baud
    pc.baud(115200);

    //Report length on HID report
    send_report.length = 63;

    //Init the motor control driver for SPI communication
    //#ifdef A4980
    //pump.initSpi();
    #ifdef A4980PWM
    pump.initSpi();
    #endif
    
    //Setting up periode for RGB LED
    red.period(0.001);
    green.period(0.001);
    blue.period(0.001);



    while(1) {
        //try to read a msg
        if(hid.readNB(&recv_report))
        {
            parseIncommingMessage();
        }

        if(startSweep)
        {
            if(pressureFloat <= (minPressureFloat + 0.02 )) 
            {
                controller.setSetPoint(maxPressureFloat);
            } 
            else if(pressureFloat >= (maxPressureFloat - 0.02))
            {
                controller.setSetPoint(minPressureFloat);
            }

        }
        else if(startTymp)
        {
            
            switch(stateTympSweep)
            {
                case 1:
                {
                    
                    if(pressureFloat <= (minPressureFloat)) 
                    {
                        controlTick.detach();
                        pump.runStepperAtSpeed(false,0,0);
                        wait(0.5);
                        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
                        controller.setSetPoint(maxPressureFloat);
                        stateTympSweep = 2;
                    }
                    
                    break;
                } // case 1    
                case 2:
                {
                    if(pressureFloat >= (maxPressureFloat)) 
                    {
                        controlTick.detach();
                        pump.runStepperAtSpeed(false,0,0);
                        wait(0.5);
                        controller.setSetPoint(0.5);
                        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
                        stateTympSweep = 3;
                    }
                    
                    break;
                } // case 2  
                case 3:
                {
                    if(pressureFloat >= 0.4995 && pressureFloat <= 0.5005) 
                    {
                        stateTympSweep = 0;
                        stopAll();
                    }
                                      
                    break;
                } // case 3
                case 4:
                {
                    //pc.printf("LEAK state");
                    //pc.printf("\r\n");

                    
                    if(pressureFloat >= (setPointFloat - 0.001)  && pressureFloat <= (setPointFloat + 0.001)) 
                    {
                        stateTympSweep = 0;
                        stopMotor();
                        
                    }
                                      
                    break;
                } // case 4    
                
            } // switch stateTympSweep
                
            
            
        }
        else if(startWear)
        {
            
            switch(stateTympSweep)
            {
                case 1:
                {
                    
                    if(pressureFloat <= (minPressureFloat + 0.01)) //tolerance on 20 daPA
                    {
                        controlTick.detach();
                        pump.runStepperAtSpeed(false,0,0);
                        wait(1);
                        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
                        controller.setSetPoint(maxPressureFloat);
                        stateTympSweep = 2;
                    }
                    
                    break;
                } // case 1    
                case 2:
                {
                    if(pressureFloat >= (maxPressureFloat - 0.01)) //tolerance on 20 daPA
                    {
                        controlTick.detach();
                        pump.runStepperAtSpeed(false,0,0);
                        wait(1);
                        controller.setSetPoint(0.5);
                        controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds)
                        stateTympSweep = 3;
                    }
                    
                    break;
                } // case 2  
                case 3:
                {
                    if(pressureFloat >= 0.49 && pressureFloat <= 0.51) //tolerance on 20 daPA
                    {
                        stateTympSweep = 0;
                        stopAll();
                        pc.printf("wear is finished!");
                        pc.printf("\r\n");
                        sendWearFinished();
                    }
                                      
                    break;
                } // case 3
                case 4:
                {
                    //pc.printf("LEAK state");
                    //pc.printf("\r\n");

                    
                    if(pressureFloat >= (setPointFloat)  ) 
                    {
                        stateTympSweep = 0;
                        pc.printf("wear leak test is finished! pressure: %f  setpoint: %f",pressureFloat, setPointFloat );
                        pc.printf("\r\n");
                        //sendWearLeakFinished();
                        stopMotor();
                        
                    }
                                      
                    break;
                } // case 4 
                 
                
            } // switch stateTympSweep
                
            
            
        }
        
#ifdef CENTER_POSITION
        
            
        
        if(pump.getStepCount() > MAX_STEPS || pump.getStepCount() < (-1*MAX_STEPS) )
        {
            stopAll();
            if(pump.getStepCount() > 0)
            {
                pumpSpeed = -MAX_SPEED;
            }
            else
            {
                pumpSpeed = MAX_SPEED;
            }
            
            //#ifdef A4980
              //pump.goToZeroPosition(pumpSpeed);
            #ifdef A4980PWM
              pump.goToZeroPosition(pumpSpeed);
            #endif
        }
#endif
       
        red = (pressureFloat > 0.505) ? 1.0 - pressureFloat : (blue = pressureFloat);
        green = (pressureFloat < 0.495) ? 1.0 - pressureFloat : (blue = pressureFloat);
        //blue = 1;
        wait(0.02);

    }
}