Ilsoe Peter
Works with pressure monitor windows application
Dependencies: ArduinoMotorShield HIH6130 Motor_shield_Arduino PID USBDevice mbed motorControl
Diff: main.cpp
- Revision:
- 0:fd07992b1c32
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Oct 06 11:57:55 2014 +0000
@@ -0,0 +1,993 @@
+ #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);
+
+ }
+}
+
+