Works with pressure monitor windows application
Dependencies: ArduinoMotorShield HIH6130 Motor_shield_Arduino PID USBDevice mbed motorControl
main.cpp
- Committer:
- piniels
- Date:
- 2014-10-06
- Revision:
- 1:e5d4451e1144
- Parent:
- 0:fd07992b1c32
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); } }