Works with pressure monitor windows application
Dependencies: ArduinoMotorShield HIH6130 Motor_shield_Arduino PID USBDevice mbed motorControl
main.cpp@1:e5d4451e1144, 2014-10-06 (annotated)
- Committer:
- piniels
- Date:
- Mon Oct 06 12:01:11 2014 +0000
- Revision:
- 1:e5d4451e1144
- Parent:
- 0:fd07992b1c32
To frank
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
piniels | 0:fd07992b1c32 | 1 | #include "mbed.h" |
piniels | 0:fd07992b1c32 | 2 | #include "USBHID.h" |
piniels | 0:fd07992b1c32 | 3 | #include "ArduinoMotorShield.h" |
piniels | 0:fd07992b1c32 | 4 | #include "motorControl.h" |
piniels | 0:fd07992b1c32 | 5 | #include "PID.h" |
piniels | 0:fd07992b1c32 | 6 | #include "Motor_shield.h" |
piniels | 0:fd07992b1c32 | 7 | #include "HIH_6130.h" |
piniels | 0:fd07992b1c32 | 8 | #include "globalt.h" |
piniels | 0:fd07992b1c32 | 9 | |
piniels | 0:fd07992b1c32 | 10 | |
piniels | 0:fd07992b1c32 | 11 | //We declare a USBHID device. By default input and output reports are 64 bytes long. |
piniels | 0:fd07992b1c32 | 12 | USBHID hid(63,63,0x03cc,0x1234,0x0001,true); |
piniels | 0:fd07992b1c32 | 13 | |
piniels | 0:fd07992b1c32 | 14 | //We declare a I2C humidity sensor of the type HIH-6130-021 |
piniels | 0:fd07992b1c32 | 15 | hih6130 humiSensor(PTC11,PTC10); |
piniels | 0:fd07992b1c32 | 16 | |
piniels | 0:fd07992b1c32 | 17 | |
piniels | 0:fd07992b1c32 | 18 | //This report will contain data to be sent |
piniels | 0:fd07992b1c32 | 19 | HID_REPORT send_report; |
piniels | 0:fd07992b1c32 | 20 | HID_REPORT send_report_humi; |
piniels | 0:fd07992b1c32 | 21 | HID_REPORT recv_report; |
piniels | 0:fd07992b1c32 | 22 | |
piniels | 0:fd07992b1c32 | 23 | //Ticker for having the correct sample rate |
piniels | 0:fd07992b1c32 | 24 | Ticker sampleTick; |
piniels | 0:fd07992b1c32 | 25 | |
piniels | 0:fd07992b1c32 | 26 | //Ticker for controlling PID controller |
piniels | 0:fd07992b1c32 | 27 | Ticker controlTick; |
piniels | 0:fd07992b1c32 | 28 | |
piniels | 0:fd07992b1c32 | 29 | |
piniels | 0:fd07992b1c32 | 30 | //Declare an analog input for pressure1sensors |
piniels | 0:fd07992b1c32 | 31 | AnalogIn pressureSensor1(PTB2); |
piniels | 0:fd07992b1c32 | 32 | AnalogIn pressureSensor2(PTB3); |
piniels | 0:fd07992b1c32 | 33 | AnalogIn pressureSensor3(PTC2); |
piniels | 0:fd07992b1c32 | 34 | //Declare an analog inputs for current sense on motor shield |
piniels | 0:fd07992b1c32 | 35 | AnalogIn sense01(PTB0); |
piniels | 0:fd07992b1c32 | 36 | AnalogIn sense02(PTB1); |
piniels | 0:fd07992b1c32 | 37 | |
piniels | 0:fd07992b1c32 | 38 | //Stepper fot controlling step motor on pump |
piniels | 0:fd07992b1c32 | 39 | int stepsPerRevolution = 200; // change this to fit the number of steps per revolution |
piniels | 0:fd07992b1c32 | 40 | |
piniels | 0:fd07992b1c32 | 41 | //RGB LED defintion |
piniels | 0:fd07992b1c32 | 42 | PwmOut red (LED_RED); |
piniels | 0:fd07992b1c32 | 43 | PwmOut green (LED_GREEN); |
piniels | 0:fd07992b1c32 | 44 | PwmOut blue (LED_BLUE); |
piniels | 0:fd07992b1c32 | 45 | |
piniels | 0:fd07992b1c32 | 46 | |
piniels | 0:fd07992b1c32 | 47 | |
piniels | 0:fd07992b1c32 | 48 | |
piniels | 0:fd07992b1c32 | 49 | |
piniels | 0:fd07992b1c32 | 50 | // initialize the motor shield library (DC motor) |
piniels | 0:fd07992b1c32 | 51 | Motor_shield m(PTA12, PTD3, PTA13); // pwm, fwd, brake |
piniels | 0:fd07992b1c32 | 52 | |
piniels | 0:fd07992b1c32 | 53 | // for your motor |
piniels | 0:fd07992b1c32 | 54 | //Initialize the stepper library: |
piniels | 0:fd07992b1c32 | 55 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 56 | 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 |
piniels | 0:fd07992b1c32 | 57 | #else |
piniels | 0:fd07992b1c32 | 58 | ArduinoMotorShield pump(stepsPerRevolution, PTA13, PTD5, PTD3, PTD1, PTA12, PTD2); |
piniels | 0:fd07992b1c32 | 59 | #endif |
piniels | 0:fd07992b1c32 | 60 | |
piniels | 0:fd07992b1c32 | 61 | |
piniels | 0:fd07992b1c32 | 62 | |
piniels | 0:fd07992b1c32 | 63 | |
piniels | 0:fd07992b1c32 | 64 | |
piniels | 0:fd07992b1c32 | 65 | //Global for the wanted pump speed |
piniels | 0:fd07992b1c32 | 66 | int pumpSpeed; |
piniels | 0:fd07992b1c32 | 67 | int minPressure; |
piniels | 0:fd07992b1c32 | 68 | int maxPressure; |
piniels | 0:fd07992b1c32 | 69 | int stateTympSweep = 0; |
piniels | 0:fd07992b1c32 | 70 | int microSteps = 0; |
piniels | 0:fd07992b1c32 | 71 | int tPauseSec = 0; |
piniels | 0:fd07992b1c32 | 72 | int tTimeOutSec = 0; |
piniels | 0:fd07992b1c32 | 73 | |
piniels | 0:fd07992b1c32 | 74 | //Global holding the humidity and temperature |
piniels | 0:fd07992b1c32 | 75 | int humiTempHolder; |
piniels | 0:fd07992b1c32 | 76 | |
piniels | 0:fd07992b1c32 | 77 | //gobal for handling one sample for the pressure1sensor |
piniels | 0:fd07992b1c32 | 78 | unsigned short pressure1= 0; |
piniels | 0:fd07992b1c32 | 79 | int pressure2= 0; |
piniels | 0:fd07992b1c32 | 80 | int pressure3= 0; |
piniels | 0:fd07992b1c32 | 81 | |
piniels | 0:fd07992b1c32 | 82 | float pressureFloat = 0; |
piniels | 0:fd07992b1c32 | 83 | float pumpSpeedFloat = 0; |
piniels | 0:fd07992b1c32 | 84 | float setpoint; |
piniels | 0:fd07992b1c32 | 85 | float minPressureFloat; |
piniels | 0:fd07992b1c32 | 86 | float maxPressureFloat; |
piniels | 0:fd07992b1c32 | 87 | float pumpSpeedSetPoint; |
piniels | 0:fd07992b1c32 | 88 | float setPointFloat; |
piniels | 0:fd07992b1c32 | 89 | bool startSweep = false; |
piniels | 0:fd07992b1c32 | 90 | bool startTymp = false; |
piniels | 0:fd07992b1c32 | 91 | bool startWear = false; |
piniels | 0:fd07992b1c32 | 92 | |
piniels | 0:fd07992b1c32 | 93 | bool dcMotor = false; |
piniels | 0:fd07992b1c32 | 94 | int reportCounter = 0; |
piniels | 0:fd07992b1c32 | 95 | |
piniels | 0:fd07992b1c32 | 96 | int current01; |
piniels | 0:fd07992b1c32 | 97 | int current02; |
piniels | 0:fd07992b1c32 | 98 | |
piniels | 0:fd07992b1c32 | 99 | |
piniels | 0:fd07992b1c32 | 100 | |
piniels | 0:fd07992b1c32 | 101 | |
piniels | 0:fd07992b1c32 | 102 | #define RATE 0.1 |
piniels | 0:fd07992b1c32 | 103 | |
piniels | 0:fd07992b1c32 | 104 | //Kc, Ti, Td, interval |
piniels | 0:fd07992b1c32 | 105 | PID controller(5.0, 5.0, 0, RATE); |
piniels | 0:fd07992b1c32 | 106 | |
piniels | 0:fd07992b1c32 | 107 | //Debug serial port |
piniels | 0:fd07992b1c32 | 108 | Serial pc(USBTX, USBRX); |
piniels | 0:fd07992b1c32 | 109 | |
piniels | 0:fd07992b1c32 | 110 | /* Tick handler for sampling of pressure1sensor */ |
piniels | 0:fd07992b1c32 | 111 | void samplepressureSensor1() |
piniels | 0:fd07992b1c32 | 112 | { |
piniels | 0:fd07992b1c32 | 113 | pressure1 = pressureSensor1.read_u16(); |
piniels | 0:fd07992b1c32 | 114 | current01 = sense01.read_u16(); |
piniels | 0:fd07992b1c32 | 115 | current02 = sense02.read_u16(); |
piniels | 0:fd07992b1c32 | 116 | |
piniels | 0:fd07992b1c32 | 117 | pressure2 = pressureSensor2.read_u16(); |
piniels | 0:fd07992b1c32 | 118 | pressure3 = pressureSensor3.read_u16(); |
piniels | 0:fd07992b1c32 | 119 | //pc.printf("Pres 3: %i ", pressure3); |
piniels | 0:fd07992b1c32 | 120 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 121 | //Fill the report with sensor data |
piniels | 0:fd07992b1c32 | 122 | if(reportCounter == 0) |
piniels | 0:fd07992b1c32 | 123 | { |
piniels | 0:fd07992b1c32 | 124 | send_report.data[reportCounter] = 'P'; |
piniels | 0:fd07992b1c32 | 125 | } |
piniels | 0:fd07992b1c32 | 126 | send_report.data[reportCounter + 1] = 0; |
piniels | 0:fd07992b1c32 | 127 | send_report.data[reportCounter + 2] = pressure1 >> 8; |
piniels | 0:fd07992b1c32 | 128 | send_report.data[reportCounter + 3] = pressure1 & 0xff; |
piniels | 0:fd07992b1c32 | 129 | send_report.data[reportCounter + 4] = 0; |
piniels | 0:fd07992b1c32 | 130 | send_report.data[reportCounter + 5] = current01 >> 8; |
piniels | 0:fd07992b1c32 | 131 | send_report.data[reportCounter + 6] = current01 & 0xff; |
piniels | 0:fd07992b1c32 | 132 | send_report.data[reportCounter + 7] = 0; |
piniels | 0:fd07992b1c32 | 133 | send_report.data[reportCounter + 8] = current02 >> 8; |
piniels | 0:fd07992b1c32 | 134 | send_report.data[reportCounter + 9] = current02 & 0xff; |
piniels | 0:fd07992b1c32 | 135 | send_report.data[reportCounter + 10] = 0; |
piniels | 0:fd07992b1c32 | 136 | send_report.data[reportCounter + 11] = pressure2 >> 8; |
piniels | 0:fd07992b1c32 | 137 | send_report.data[reportCounter + 12] = pressure2 & 0xff; |
piniels | 0:fd07992b1c32 | 138 | send_report.data[reportCounter + 13] = 0; |
piniels | 0:fd07992b1c32 | 139 | send_report.data[reportCounter + 14] = pressure3 >> 8; |
piniels | 0:fd07992b1c32 | 140 | send_report.data[reportCounter + 15] = pressure3 & 0xff; |
piniels | 0:fd07992b1c32 | 141 | //send_report.data[reportCounter + 16] = 0; |
piniels | 0:fd07992b1c32 | 142 | reportCounter += 15; //Counting how many of the 63 bytes is used |
piniels | 0:fd07992b1c32 | 143 | send_report.length = 63; |
piniels | 0:fd07992b1c32 | 144 | if(reportCounter >= 60) { |
piniels | 0:fd07992b1c32 | 145 | //Send the report |
piniels | 0:fd07992b1c32 | 146 | hid.sendNB(&send_report); |
piniels | 0:fd07992b1c32 | 147 | reportCounter = 0; |
piniels | 0:fd07992b1c32 | 148 | } |
piniels | 0:fd07992b1c32 | 149 | |
piniels | 0:fd07992b1c32 | 150 | } |
piniels | 0:fd07992b1c32 | 151 | |
piniels | 0:fd07992b1c32 | 152 | /* Tick handler computing of HID controller */ |
piniels | 0:fd07992b1c32 | 153 | void computeHIDController() |
piniels | 0:fd07992b1c32 | 154 | { |
piniels | 0:fd07992b1c32 | 155 | pressureFloat = pressureSensor1.read(); |
piniels | 0:fd07992b1c32 | 156 | |
piniels | 0:fd07992b1c32 | 157 | |
piniels | 0:fd07992b1c32 | 158 | //Update the process variable. |
piniels | 0:fd07992b1c32 | 159 | controller.setProcessValue(pressureFloat); |
piniels | 0:fd07992b1c32 | 160 | //Set the new output. |
piniels | 0:fd07992b1c32 | 161 | pumpSpeedFloat = controller.compute(); |
piniels | 0:fd07992b1c32 | 162 | |
piniels | 0:fd07992b1c32 | 163 | |
piniels | 0:fd07992b1c32 | 164 | if(dcMotor) { |
piniels | 0:fd07992b1c32 | 165 | |
piniels | 0:fd07992b1c32 | 166 | m.speed(pumpSpeedFloat); |
piniels | 0:fd07992b1c32 | 167 | //pc.printf("2 DC speed: %f ", pumpSpeedFloat); |
piniels | 0:fd07992b1c32 | 168 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 169 | } else { |
piniels | 0:fd07992b1c32 | 170 | if(pumpSpeedFloat < 0) { |
piniels | 0:fd07992b1c32 | 171 | //if(pumpSpeedFloat > -15) { |
piniels | 0:fd07992b1c32 | 172 | // pumpSpeedFloat = -15; |
piniels | 0:fd07992b1c32 | 173 | //} |
piniels | 0:fd07992b1c32 | 174 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 175 | //pump.runStepperAtSpeedWithSPI(true,(pumpSpeedFloat*(-1)), 0); |
piniels | 0:fd07992b1c32 | 176 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 177 | pump.runStepperAtSpeed(true,(pumpSpeedFloat*(-1)), 0); |
piniels | 0:fd07992b1c32 | 178 | #else |
piniels | 0:fd07992b1c32 | 179 | pump.runStepperAtSpeed(true,(pumpSpeedFloat*(-1)), 0); |
piniels | 0:fd07992b1c32 | 180 | #endif |
piniels | 0:fd07992b1c32 | 181 | |
piniels | 0:fd07992b1c32 | 182 | } else { |
piniels | 0:fd07992b1c32 | 183 | //if(pumpSpeedFloat < 15) { |
piniels | 0:fd07992b1c32 | 184 | // pumpSpeedFloat = 15; |
piniels | 0:fd07992b1c32 | 185 | //} |
piniels | 0:fd07992b1c32 | 186 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 187 | //pump.runStepperAtSpeedWithSPI(true,pumpSpeedFloat, 1); |
piniels | 0:fd07992b1c32 | 188 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 189 | pump.runStepperAtSpeed(true,pumpSpeedFloat, 1); |
piniels | 0:fd07992b1c32 | 190 | #else |
piniels | 0:fd07992b1c32 | 191 | pump.runStepperAtSpeed(true,pumpSpeedFloat, 1); |
piniels | 0:fd07992b1c32 | 192 | #endif |
piniels | 0:fd07992b1c32 | 193 | } |
piniels | 0:fd07992b1c32 | 194 | //pc.printf("Stepper speed: %f : %f ", pumpSpeedFloat,pressureFloat); |
piniels | 0:fd07992b1c32 | 195 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 196 | } |
piniels | 0:fd07992b1c32 | 197 | |
piniels | 0:fd07992b1c32 | 198 | |
piniels | 0:fd07992b1c32 | 199 | |
piniels | 0:fd07992b1c32 | 200 | |
piniels | 0:fd07992b1c32 | 201 | } |
piniels | 0:fd07992b1c32 | 202 | |
piniels | 0:fd07992b1c32 | 203 | float convertDaPAToNominalFloat(int daPa) |
piniels | 0:fd07992b1c32 | 204 | { |
piniels | 0:fd07992b1c32 | 205 | //pc.printf("daPA: %d ", daPa); |
piniels | 0:fd07992b1c32 | 206 | float kPa = ((float) daPa)/100; |
piniels | 0:fd07992b1c32 | 207 | //pc.printf("kPA: %f ", kPa); |
piniels | 0:fd07992b1c32 | 208 | float returnValue; |
piniels | 0:fd07992b1c32 | 209 | returnValue = (0.057*kPa)+0.5; |
piniels | 0:fd07992b1c32 | 210 | return returnValue; |
piniels | 0:fd07992b1c32 | 211 | } |
piniels | 0:fd07992b1c32 | 212 | |
piniels | 0:fd07992b1c32 | 213 | /* Sample analog input and send on HID */ |
piniels | 0:fd07992b1c32 | 214 | void samplePressureSensors(int averageNum) |
piniels | 0:fd07992b1c32 | 215 | { |
piniels | 0:fd07992b1c32 | 216 | int averPressure1 = 0; |
piniels | 0:fd07992b1c32 | 217 | int averCurrent01 = 0; |
piniels | 0:fd07992b1c32 | 218 | int averCurrent02 = 0; |
piniels | 0:fd07992b1c32 | 219 | |
piniels | 0:fd07992b1c32 | 220 | int averPressure2 = 0; |
piniels | 0:fd07992b1c32 | 221 | int averPressure3 = 0; |
piniels | 0:fd07992b1c32 | 222 | |
piniels | 0:fd07992b1c32 | 223 | |
piniels | 0:fd07992b1c32 | 224 | int i = 0; |
piniels | 0:fd07992b1c32 | 225 | for(i = 0; i < averageNum; i++) |
piniels | 0:fd07992b1c32 | 226 | { |
piniels | 0:fd07992b1c32 | 227 | averPressure1 += pressureSensor1.read_u16(); |
piniels | 0:fd07992b1c32 | 228 | averCurrent01 += sense01.read_u16(); |
piniels | 0:fd07992b1c32 | 229 | averCurrent02 += sense02.read_u16(); |
piniels | 0:fd07992b1c32 | 230 | |
piniels | 0:fd07992b1c32 | 231 | averPressure2 += pressureSensor2.read_u16(); |
piniels | 0:fd07992b1c32 | 232 | averPressure3 += pressureSensor3.read_u16(); |
piniels | 0:fd07992b1c32 | 233 | wait(0.001); |
piniels | 0:fd07992b1c32 | 234 | } |
piniels | 0:fd07992b1c32 | 235 | |
piniels | 0:fd07992b1c32 | 236 | |
piniels | 0:fd07992b1c32 | 237 | averPressure1 = averPressure1/averageNum; |
piniels | 0:fd07992b1c32 | 238 | averCurrent01 = averCurrent01/averageNum; |
piniels | 0:fd07992b1c32 | 239 | averCurrent02 = averCurrent02/averageNum; |
piniels | 0:fd07992b1c32 | 240 | |
piniels | 0:fd07992b1c32 | 241 | averPressure2 = averPressure2/averageNum; |
piniels | 0:fd07992b1c32 | 242 | averPressure3 = averPressure3/averageNum; |
piniels | 0:fd07992b1c32 | 243 | |
piniels | 0:fd07992b1c32 | 244 | |
piniels | 0:fd07992b1c32 | 245 | //Fill the report with sensor data |
piniels | 0:fd07992b1c32 | 246 | send_report.data[0] = 'S'; |
piniels | 0:fd07992b1c32 | 247 | send_report.data[1] = 0; |
piniels | 0:fd07992b1c32 | 248 | send_report.data[2] = averPressure1 >> 8; |
piniels | 0:fd07992b1c32 | 249 | send_report.data[3] = averPressure1 & 0xff; |
piniels | 0:fd07992b1c32 | 250 | send_report.data[4] = 0; |
piniels | 0:fd07992b1c32 | 251 | send_report.data[5] = averCurrent01 >> 8; |
piniels | 0:fd07992b1c32 | 252 | send_report.data[6] = averCurrent01 & 0xff; |
piniels | 0:fd07992b1c32 | 253 | send_report.data[7] = 0; |
piniels | 0:fd07992b1c32 | 254 | send_report.data[8] = averCurrent02 >> 8; |
piniels | 0:fd07992b1c32 | 255 | send_report.data[9] = averCurrent02 & 0xff; |
piniels | 0:fd07992b1c32 | 256 | send_report.data[10] = 0; |
piniels | 0:fd07992b1c32 | 257 | send_report.data[11] = averPressure2 >> 8; |
piniels | 0:fd07992b1c32 | 258 | send_report.data[12] = averPressure2 & 0xff; |
piniels | 0:fd07992b1c32 | 259 | send_report.data[13] = 0; |
piniels | 0:fd07992b1c32 | 260 | send_report.data[14] = averPressure3 >> 8; |
piniels | 0:fd07992b1c32 | 261 | send_report.data[15] = averPressure3 & 0xff; |
piniels | 0:fd07992b1c32 | 262 | send_report.data[16] = 0; |
piniels | 0:fd07992b1c32 | 263 | send_report.length = 63; |
piniels | 0:fd07992b1c32 | 264 | hid.sendNB(&send_report); |
piniels | 0:fd07992b1c32 | 265 | |
piniels | 0:fd07992b1c32 | 266 | |
piniels | 0:fd07992b1c32 | 267 | |
piniels | 0:fd07992b1c32 | 268 | } |
piniels | 0:fd07992b1c32 | 269 | |
piniels | 0:fd07992b1c32 | 270 | void stopAll() |
piniels | 0:fd07992b1c32 | 271 | { |
piniels | 0:fd07992b1c32 | 272 | sampleTick.detach(); |
piniels | 0:fd07992b1c32 | 273 | controlTick.detach(); |
piniels | 0:fd07992b1c32 | 274 | pump.openCloseValve(true); |
piniels | 0:fd07992b1c32 | 275 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 276 | //pump.runStepperAtSpeedWithSPI(false,0,0); |
piniels | 0:fd07992b1c32 | 277 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 278 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 279 | #else |
piniels | 0:fd07992b1c32 | 280 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 281 | #endif |
piniels | 0:fd07992b1c32 | 282 | m.speed(0); |
piniels | 0:fd07992b1c32 | 283 | startSweep = false; |
piniels | 0:fd07992b1c32 | 284 | startTymp = false; |
piniels | 0:fd07992b1c32 | 285 | startWear = false; |
piniels | 0:fd07992b1c32 | 286 | reportCounter = 0; |
piniels | 0:fd07992b1c32 | 287 | pressureFloat = 0.5; |
piniels | 0:fd07992b1c32 | 288 | pc.printf("Stop"); |
piniels | 0:fd07992b1c32 | 289 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 290 | } |
piniels | 0:fd07992b1c32 | 291 | |
piniels | 0:fd07992b1c32 | 292 | void stopMotor() |
piniels | 0:fd07992b1c32 | 293 | { |
piniels | 0:fd07992b1c32 | 294 | //sampleTick.detach(); |
piniels | 0:fd07992b1c32 | 295 | controlTick.detach(); |
piniels | 0:fd07992b1c32 | 296 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 297 | //pump.runStepperAtSpeedWithSPI(false,0,0); |
piniels | 0:fd07992b1c32 | 298 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 299 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 300 | #else |
piniels | 0:fd07992b1c32 | 301 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 302 | #endif |
piniels | 0:fd07992b1c32 | 303 | m.speed(0); |
piniels | 0:fd07992b1c32 | 304 | |
piniels | 0:fd07992b1c32 | 305 | } |
piniels | 0:fd07992b1c32 | 306 | |
piniels | 0:fd07992b1c32 | 307 | void sendWearFinished() |
piniels | 0:fd07992b1c32 | 308 | { |
piniels | 0:fd07992b1c32 | 309 | //Fill the report with sensor data |
piniels | 0:fd07992b1c32 | 310 | send_report.data[0] = 'W'; |
piniels | 0:fd07992b1c32 | 311 | send_report.data[1] = 0; |
piniels | 0:fd07992b1c32 | 312 | send_report.data[2] = 'F'; |
piniels | 0:fd07992b1c32 | 313 | send_report.data[1] = 0; |
piniels | 0:fd07992b1c32 | 314 | send_report.length = 63; |
piniels | 0:fd07992b1c32 | 315 | hid.sendNB(&send_report); |
piniels | 0:fd07992b1c32 | 316 | } |
piniels | 0:fd07992b1c32 | 317 | |
piniels | 0:fd07992b1c32 | 318 | void sendWearLeakFinished() |
piniels | 0:fd07992b1c32 | 319 | { |
piniels | 0:fd07992b1c32 | 320 | //Fill the report with sensor data |
piniels | 0:fd07992b1c32 | 321 | send_report.data[0] = 'W'; |
piniels | 0:fd07992b1c32 | 322 | send_report.data[1] = 0; |
piniels | 0:fd07992b1c32 | 323 | send_report.data[2] = 'L'; |
piniels | 0:fd07992b1c32 | 324 | send_report.data[1] = 0; |
piniels | 0:fd07992b1c32 | 325 | send_report.length = 63; |
piniels | 0:fd07992b1c32 | 326 | hid.sendNB(&send_report); |
piniels | 0:fd07992b1c32 | 327 | } |
piniels | 0:fd07992b1c32 | 328 | |
piniels | 0:fd07992b1c32 | 329 | //Parser for HID reports |
piniels | 0:fd07992b1c32 | 330 | void parseIncommingMessage( ) |
piniels | 0:fd07992b1c32 | 331 | { |
piniels | 0:fd07992b1c32 | 332 | int setPointMessage; |
piniels | 0:fd07992b1c32 | 333 | float setPointMessageFloat; |
piniels | 0:fd07992b1c32 | 334 | //-------------------------------------------------------STOP-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 335 | if(recv_report.data[0] == 'S' && recv_report.data[1] == 'T' && recv_report.data[2] == 'O' && recv_report.data[3] == 'P') { |
piniels | 0:fd07992b1c32 | 336 | stopAll(); |
piniels | 0:fd07992b1c32 | 337 | } |
piniels | 0:fd07992b1c32 | 338 | //-------------------------------------------------------STOP MOTOR-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 339 | if(recv_report.data[0] == 'S' && recv_report.data[1] == 'T' && recv_report.data[2] == 'M' && recv_report.data[3] == 'O') { |
piniels | 0:fd07992b1c32 | 340 | stopMotor(); |
piniels | 0:fd07992b1c32 | 341 | } |
piniels | 0:fd07992b1c32 | 342 | //-------------------------------------------------------SET OUTPUT PIN-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 343 | if(recv_report.data[0] == 'O' && recv_report.data[1] == 'U' && recv_report.data[2] == 'T' && recv_report.data[3] == 'P') { |
piniels | 0:fd07992b1c32 | 344 | pc.printf("Set Output"); |
piniels | 0:fd07992b1c32 | 345 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 346 | if(recv_report.data[5] == 'F') |
piniels | 0:fd07992b1c32 | 347 | { |
piniels | 0:fd07992b1c32 | 348 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 349 | pc.printf("True"); |
piniels | 0:fd07992b1c32 | 350 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 351 | } |
piniels | 0:fd07992b1c32 | 352 | else |
piniels | 0:fd07992b1c32 | 353 | { |
piniels | 0:fd07992b1c32 | 354 | pump.openCloseValve(true); |
piniels | 0:fd07992b1c32 | 355 | pc.printf("False"); |
piniels | 0:fd07992b1c32 | 356 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 357 | } |
piniels | 0:fd07992b1c32 | 358 | |
piniels | 0:fd07992b1c32 | 359 | } |
piniels | 0:fd07992b1c32 | 360 | //-------------------------------------------------------SWEEP-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 361 | 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') { |
piniels | 0:fd07992b1c32 | 362 | pumpSpeed = (((int)(recv_report.data[6]) - 0x30) * 100) + (((int)(recv_report.data[7]) - 0x30) * 10) + (((int)(recv_report.data[8]) - 0x30)); |
piniels | 0:fd07992b1c32 | 363 | minPressure = -1*(((recv_report.data[10] - 0x30) * 100) + ((recv_report.data[11] - 0x30) * 10) + ((recv_report.data[12] - 0x30))); |
piniels | 0:fd07992b1c32 | 364 | maxPressure = ((recv_report.data[14] - 0x30) * 100) + ((recv_report.data[15] - 0x30) * 10) + ((recv_report.data[16] - 0x30)); |
piniels | 0:fd07992b1c32 | 365 | minPressureFloat = convertDaPAToNominalFloat(minPressure); |
piniels | 0:fd07992b1c32 | 366 | maxPressureFloat = convertDaPAToNominalFloat(maxPressure); |
piniels | 0:fd07992b1c32 | 367 | |
piniels | 0:fd07992b1c32 | 368 | //True if DC motor |
piniels | 0:fd07992b1c32 | 369 | if(recv_report.data[18] == 'T' && recv_report.data[19] == 'r' && recv_report.data[20] == 'u' && recv_report.data[21] == 'e') { |
piniels | 0:fd07992b1c32 | 370 | dcMotor = true; |
piniels | 0:fd07992b1c32 | 371 | //float pumpspeedNorm = ((( (float) pumpSpeed-15)/6.75)/24); /// HACK for testing pump from WPM |
piniels | 0:fd07992b1c32 | 372 | controller.setBias(0.3333); |
piniels | 0:fd07992b1c32 | 373 | controller.setOutputLimits(-1, 1); |
piniels | 0:fd07992b1c32 | 374 | microSteps = (recv_report.data[23] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 375 | // pc.printf("DC Motor: %d",pumpspeedNorm); |
piniels | 0:fd07992b1c32 | 376 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 377 | } else { |
piniels | 0:fd07992b1c32 | 378 | dcMotor = false; |
piniels | 0:fd07992b1c32 | 379 | //RPM output (stepper motor) from -400.0 to 400.0 |
piniels | 0:fd07992b1c32 | 380 | controller.setBias(2.0); |
piniels | 0:fd07992b1c32 | 381 | controller.setOutputLimits(-pumpSpeed, pumpSpeed); |
piniels | 0:fd07992b1c32 | 382 | microSteps = (recv_report.data[24] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 383 | //pc.printf("Stepper Motor"); |
piniels | 0:fd07992b1c32 | 384 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 385 | |
piniels | 0:fd07992b1c32 | 386 | } |
piniels | 0:fd07992b1c32 | 387 | |
piniels | 0:fd07992b1c32 | 388 | |
piniels | 0:fd07992b1c32 | 389 | pc.printf("MicroSteps: %i", microSteps); |
piniels | 0:fd07992b1c32 | 390 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 391 | |
piniels | 0:fd07992b1c32 | 392 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 393 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 394 | pc.printf("Get MicroSteps: %i", pump.getMicroSteps()); |
piniels | 0:fd07992b1c32 | 395 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 396 | #endif |
piniels | 0:fd07992b1c32 | 397 | |
piniels | 0:fd07992b1c32 | 398 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 399 | controller.setSetPoint(minPressureFloat); |
piniels | 0:fd07992b1c32 | 400 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 401 | sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds) |
piniels | 0:fd07992b1c32 | 402 | startSweep = true; |
piniels | 0:fd07992b1c32 | 403 | startTymp = false; |
piniels | 0:fd07992b1c32 | 404 | startWear = false; |
piniels | 0:fd07992b1c32 | 405 | } |
piniels | 0:fd07992b1c32 | 406 | //-------------------------------------------------------Const Presssure-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 407 | else if(recv_report.data[0] == 'P' && recv_report.data[1] == 'R' && recv_report.data[2] == 'E' && recv_report.data[3] == 'S' ) { |
piniels | 0:fd07992b1c32 | 408 | startSweep = false; |
piniels | 0:fd07992b1c32 | 409 | pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 410 | |
piniels | 0:fd07992b1c32 | 411 | //check if negative pressure1is wanted |
piniels | 0:fd07992b1c32 | 412 | int negativevalue = 0; |
piniels | 0:fd07992b1c32 | 413 | if(recv_report.data[9] == '-') { |
piniels | 0:fd07992b1c32 | 414 | negativevalue = 1; |
piniels | 0:fd07992b1c32 | 415 | 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)); |
piniels | 0:fd07992b1c32 | 416 | } else { |
piniels | 0:fd07992b1c32 | 417 | setPointMessage = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30)); |
piniels | 0:fd07992b1c32 | 418 | } |
piniels | 0:fd07992b1c32 | 419 | |
piniels | 0:fd07992b1c32 | 420 | |
piniels | 0:fd07992b1c32 | 421 | setPointMessageFloat = convertDaPAToNominalFloat(setPointMessage); |
piniels | 0:fd07992b1c32 | 422 | |
piniels | 0:fd07992b1c32 | 423 | //True if DC motor |
piniels | 0:fd07992b1c32 | 424 | 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') { |
piniels | 0:fd07992b1c32 | 425 | dcMotor = true; |
piniels | 0:fd07992b1c32 | 426 | controller.setOutputLimits(-1.0, 1.0); //full output limits |
piniels | 0:fd07992b1c32 | 427 | controller.setBias(0.0); |
piniels | 0:fd07992b1c32 | 428 | microSteps = (recv_report.data[18] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 429 | // pc.printf("DC Motor"); |
piniels | 0:fd07992b1c32 | 430 | // pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 431 | } else { |
piniels | 0:fd07992b1c32 | 432 | dcMotor = false; |
piniels | 0:fd07992b1c32 | 433 | controller.setBias(2.0); |
piniels | 0:fd07992b1c32 | 434 | //RPM output (stepper motor) from -400.0 to 400.0 |
piniels | 0:fd07992b1c32 | 435 | controller.setOutputLimits(-pumpSpeed, pumpSpeed); |
piniels | 0:fd07992b1c32 | 436 | microSteps = (recv_report.data[19] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 437 | // pc.printf("Stepper Motor"); |
piniels | 0:fd07992b1c32 | 438 | // pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 439 | } |
piniels | 0:fd07992b1c32 | 440 | |
piniels | 0:fd07992b1c32 | 441 | pc.printf("MicroSteps: %i", microSteps); |
piniels | 0:fd07992b1c32 | 442 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 443 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 444 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 445 | #endif |
piniels | 0:fd07992b1c32 | 446 | pc.printf("CONST PRES SETPOINT: %i %f ",setPointMessage,setPointMessageFloat); |
piniels | 0:fd07992b1c32 | 447 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 448 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 449 | controller.setSetPoint(setPointMessageFloat); |
piniels | 0:fd07992b1c32 | 450 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 451 | sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds) |
piniels | 0:fd07992b1c32 | 452 | |
piniels | 0:fd07992b1c32 | 453 | |
piniels | 0:fd07992b1c32 | 454 | } |
piniels | 0:fd07992b1c32 | 455 | //-------------------------------------------------------LEAK-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 456 | else if(recv_report.data[0] == 'L' && recv_report.data[1] == 'E' && recv_report.data[2] == 'A' && recv_report.data[3] == 'K' ) { |
piniels | 0:fd07992b1c32 | 457 | startSweep = false; |
piniels | 0:fd07992b1c32 | 458 | pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 459 | |
piniels | 0:fd07992b1c32 | 460 | //check if negative pressure1is wanted |
piniels | 0:fd07992b1c32 | 461 | int negativevalue = 0; |
piniels | 0:fd07992b1c32 | 462 | if(recv_report.data[9] == '-') { |
piniels | 0:fd07992b1c32 | 463 | negativevalue = 1; |
piniels | 0:fd07992b1c32 | 464 | 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)); |
piniels | 0:fd07992b1c32 | 465 | } else { |
piniels | 0:fd07992b1c32 | 466 | setPointMessage = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30)); |
piniels | 0:fd07992b1c32 | 467 | } |
piniels | 0:fd07992b1c32 | 468 | |
piniels | 0:fd07992b1c32 | 469 | |
piniels | 0:fd07992b1c32 | 470 | setPointMessageFloat = convertDaPAToNominalFloat(setPointMessage); |
piniels | 0:fd07992b1c32 | 471 | |
piniels | 0:fd07992b1c32 | 472 | //True if DC motor |
piniels | 0:fd07992b1c32 | 473 | 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') { |
piniels | 0:fd07992b1c32 | 474 | dcMotor = true; |
piniels | 0:fd07992b1c32 | 475 | controller.setOutputLimits(-1.0, 1.0); //full output limits |
piniels | 0:fd07992b1c32 | 476 | controller.setBias(0.0); |
piniels | 0:fd07992b1c32 | 477 | // pc.printf("DC Motor"); |
piniels | 0:fd07992b1c32 | 478 | // pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 479 | } else { |
piniels | 0:fd07992b1c32 | 480 | dcMotor = false; |
piniels | 0:fd07992b1c32 | 481 | controller.setBias(2.0); |
piniels | 0:fd07992b1c32 | 482 | //RPM output (stepper motor) from -400.0 to 400.0 |
piniels | 0:fd07992b1c32 | 483 | controller.setOutputLimits(-pumpSpeed, pumpSpeed); |
piniels | 0:fd07992b1c32 | 484 | // pc.printf("Stepper Motor"); |
piniels | 0:fd07992b1c32 | 485 | // pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 486 | } |
piniels | 0:fd07992b1c32 | 487 | pc.printf("LEAK CONST PRES SETPOINT: %i %f ",setPointMessage,setPointMessageFloat); |
piniels | 0:fd07992b1c32 | 488 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 489 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 490 | controller.setSetPoint(setPointMessageFloat); |
piniels | 0:fd07992b1c32 | 491 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 492 | sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds) |
piniels | 0:fd07992b1c32 | 493 | stateTympSweep = 4; |
piniels | 0:fd07992b1c32 | 494 | setPointFloat = setPointMessageFloat; |
piniels | 0:fd07992b1c32 | 495 | startTymp = true; |
piniels | 0:fd07992b1c32 | 496 | startSweep = false; |
piniels | 0:fd07992b1c32 | 497 | startWear = false; |
piniels | 0:fd07992b1c32 | 498 | pc.printf("LEAK start"); |
piniels | 0:fd07992b1c32 | 499 | |
piniels | 0:fd07992b1c32 | 500 | |
piniels | 0:fd07992b1c32 | 501 | } |
piniels | 0:fd07992b1c32 | 502 | //-------------------------------------------------------Humidity-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 503 | else if(recv_report.data[0] == 'H' && recv_report.data[1] == 'U' && recv_report.data[2] == 'M' && recv_report.data[3] == 'I' ) |
piniels | 0:fd07992b1c32 | 504 | { |
piniels | 0:fd07992b1c32 | 505 | pc.printf("HUMI"); |
piniels | 0:fd07992b1c32 | 506 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 507 | int data = humiSensor.fetchDataRaw(); |
piniels | 0:fd07992b1c32 | 508 | send_report.data[0] = 'H'; |
piniels | 0:fd07992b1c32 | 509 | send_report.data[1] = 0; |
piniels | 0:fd07992b1c32 | 510 | send_report.data[2] = (data & 0xff000000) >> 24; |
piniels | 0:fd07992b1c32 | 511 | send_report.data[3] = (data & 0xff0000) >> 16; |
piniels | 0:fd07992b1c32 | 512 | send_report.data[4] = (data & 0xff00) >> 8; |
piniels | 0:fd07992b1c32 | 513 | send_report.data[5] = (data & 0xff); |
piniels | 0:fd07992b1c32 | 514 | send_report.data[6] = 0; |
piniels | 0:fd07992b1c32 | 515 | send_report.length = 63; |
piniels | 0:fd07992b1c32 | 516 | |
piniels | 0:fd07992b1c32 | 517 | //Send the report |
piniels | 0:fd07992b1c32 | 518 | hid.send(&send_report); |
piniels | 0:fd07992b1c32 | 519 | } |
piniels | 0:fd07992b1c32 | 520 | //-------------------------------------------------------Sensor measurement-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 521 | else if(recv_report.data[0] == 'S' && recv_report.data[1] == 'E' && recv_report.data[2] == 'N' && recv_report.data[3] == 'S' ) |
piniels | 0:fd07992b1c32 | 522 | { |
piniels | 0:fd07992b1c32 | 523 | //pc.printf("SENS"); |
piniels | 0:fd07992b1c32 | 524 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 525 | int averageNum = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 526 | //pc.printf("AverageNum %i",averageNum); |
piniels | 0:fd07992b1c32 | 527 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 528 | samplePressureSensors(averageNum); |
piniels | 0:fd07992b1c32 | 529 | } |
piniels | 0:fd07992b1c32 | 530 | //-------------------------------------------------------RMP-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 531 | else if(recv_report.data[0] == 'R' && recv_report.data[1] == 'P' && recv_report.data[2] == 'M' && recv_report.data[3] == 'S' ) |
piniels | 0:fd07992b1c32 | 532 | { |
piniels | 0:fd07992b1c32 | 533 | pc.printf("RPMS"); |
piniels | 0:fd07992b1c32 | 534 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 535 | startSweep = false; |
piniels | 0:fd07992b1c32 | 536 | //check if negative pressure1is wanted |
piniels | 0:fd07992b1c32 | 537 | int negativevalue = 0; |
piniels | 0:fd07992b1c32 | 538 | if(recv_report.data[5] == '-') |
piniels | 0:fd07992b1c32 | 539 | { |
piniels | 0:fd07992b1c32 | 540 | negativevalue = 1; |
piniels | 0:fd07992b1c32 | 541 | pumpSpeed = (((int)(recv_report.data[negativevalue + 5]) - 0x30) * 100) + (((int)(recv_report.data[negativevalue + 6]) - 0x30) * 10) + (((int)(recv_report.data[negativevalue + 7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 542 | pumpSpeed = -1*pumpSpeed; |
piniels | 0:fd07992b1c32 | 543 | microSteps = (recv_report.data[10] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 544 | } |
piniels | 0:fd07992b1c32 | 545 | else |
piniels | 0:fd07992b1c32 | 546 | { |
piniels | 0:fd07992b1c32 | 547 | pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 548 | microSteps = (recv_report.data[9] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 549 | } |
piniels | 0:fd07992b1c32 | 550 | pc.printf("RPMS pumps %i", pumpSpeed); |
piniels | 0:fd07992b1c32 | 551 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 552 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 553 | pc.printf("MicroSteps: %i", microSteps); |
piniels | 0:fd07992b1c32 | 554 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 555 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 556 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 557 | #endif |
piniels | 0:fd07992b1c32 | 558 | if(pumpSpeed < 0) { |
piniels | 0:fd07992b1c32 | 559 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 560 | //pump.runStepperAtSpeedWithSPI(true,(pumpSpeed*(-1)), 0); |
piniels | 0:fd07992b1c32 | 561 | #ifdef A4890PWM |
piniels | 0:fd07992b1c32 | 562 | pump.doRamp(10,pump.calculateStepDelay(pumpSpeed*(-1))); |
piniels | 0:fd07992b1c32 | 563 | pump.runStepperAtSpeed(true,(pumpSpeed*(-1)), 0); |
piniels | 0:fd07992b1c32 | 564 | #else |
piniels | 0:fd07992b1c32 | 565 | pump.runStepperAtSpeed(true,(pumpSpeed*(-1)), 0); |
piniels | 0:fd07992b1c32 | 566 | #endif |
piniels | 0:fd07992b1c32 | 567 | } else { |
piniels | 0:fd07992b1c32 | 568 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 569 | //pump.runStepperAtSpeedWithSPI(true,pumpSpeed, 1); |
piniels | 0:fd07992b1c32 | 570 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 571 | pump.doRamp(10,pump.calculateStepDelay(pumpSpeed)); |
piniels | 0:fd07992b1c32 | 572 | pump.runStepperAtSpeed(true,pumpSpeed, 1); |
piniels | 0:fd07992b1c32 | 573 | #else |
piniels | 0:fd07992b1c32 | 574 | pump.runStepperAtSpeed(true,pumpSpeed, 1); |
piniels | 0:fd07992b1c32 | 575 | #endif |
piniels | 0:fd07992b1c32 | 576 | } |
piniels | 0:fd07992b1c32 | 577 | } |
piniels | 0:fd07992b1c32 | 578 | //-------------------------------------------------------STEP-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 579 | else if(recv_report.data[0] == 'S' && recv_report.data[1] == 'T' && recv_report.data[2] == 'E' && recv_report.data[3] == 'P' ) |
piniels | 0:fd07992b1c32 | 580 | { |
piniels | 0:fd07992b1c32 | 581 | pc.printf("STEP"); |
piniels | 0:fd07992b1c32 | 582 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 583 | startSweep = false; |
piniels | 0:fd07992b1c32 | 584 | //check if negative pressure1is wanted |
piniels | 0:fd07992b1c32 | 585 | int negativevalue = 0; |
piniels | 0:fd07992b1c32 | 586 | int numOfSteps = 0; |
piniels | 0:fd07992b1c32 | 587 | int delayBetweenSteps = 0; |
piniels | 0:fd07992b1c32 | 588 | bool direction = true; |
piniels | 0:fd07992b1c32 | 589 | if(recv_report.data[5] == '-') |
piniels | 0:fd07992b1c32 | 590 | { |
piniels | 0:fd07992b1c32 | 591 | negativevalue = 1; |
piniels | 0:fd07992b1c32 | 592 | numOfSteps = (((int)(recv_report.data[negativevalue + 5]) - 0x30) * 100) + (((int)(recv_report.data[negativevalue + 6]) - 0x30) * 10) + (((int)(recv_report.data[negativevalue + 7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 593 | direction = false; |
piniels | 0:fd07992b1c32 | 594 | delayBetweenSteps = (((int)(recv_report.data[negativevalue + 9]) - 0x30) * 100) + (((int)(recv_report.data[negativevalue + 10]) - 0x30) * 10) + (((int)(recv_report.data[negativevalue + 11]) - 0x30)); |
piniels | 0:fd07992b1c32 | 595 | microSteps = (recv_report.data[14] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 596 | } |
piniels | 0:fd07992b1c32 | 597 | else |
piniels | 0:fd07992b1c32 | 598 | { |
piniels | 0:fd07992b1c32 | 599 | numOfSteps = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 600 | delayBetweenSteps = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30)); |
piniels | 0:fd07992b1c32 | 601 | microSteps = (recv_report.data[13] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 602 | } |
piniels | 0:fd07992b1c32 | 603 | pc.printf("STEP numOfSteps %i delay %i microsteps %i", numOfSteps,delayBetweenSteps,microSteps); |
piniels | 0:fd07992b1c32 | 604 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 605 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 606 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 607 | #endif |
piniels | 0:fd07992b1c32 | 608 | if(direction) { |
piniels | 0:fd07992b1c32 | 609 | |
piniels | 0:fd07992b1c32 | 610 | //pump.doRamp(10,pump.calculateStepDelay(pumpSpeed*(-1))); |
piniels | 0:fd07992b1c32 | 611 | pump.step(numOfSteps,delayBetweenSteps,true); |
piniels | 0:fd07992b1c32 | 612 | |
piniels | 0:fd07992b1c32 | 613 | |
piniels | 0:fd07992b1c32 | 614 | } else { |
piniels | 0:fd07992b1c32 | 615 | pump.step(numOfSteps,delayBetweenSteps,false); |
piniels | 0:fd07992b1c32 | 616 | } |
piniels | 0:fd07992b1c32 | 617 | } |
piniels | 0:fd07992b1c32 | 618 | |
piniels | 0:fd07992b1c32 | 619 | //-------------------------------------------------------ZERO Posion-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 620 | else if(recv_report.data[0] == 'Z' && recv_report.data[1] == 'E' && recv_report.data[2] == 'R' && recv_report.data[3] == 'O' ) |
piniels | 0:fd07992b1c32 | 621 | { |
piniels | 0:fd07992b1c32 | 622 | pc.printf("ZERO"); |
piniels | 0:fd07992b1c32 | 623 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 624 | startSweep = false; |
piniels | 0:fd07992b1c32 | 625 | startTymp = false; |
piniels | 0:fd07992b1c32 | 626 | stopAll(); |
piniels | 0:fd07992b1c32 | 627 | if(pump.getStepCount() > 0) |
piniels | 0:fd07992b1c32 | 628 | { |
piniels | 0:fd07992b1c32 | 629 | pumpSpeed = -MAX_SPEED; |
piniels | 0:fd07992b1c32 | 630 | } |
piniels | 0:fd07992b1c32 | 631 | else |
piniels | 0:fd07992b1c32 | 632 | { |
piniels | 0:fd07992b1c32 | 633 | pumpSpeed = MAX_SPEED; |
piniels | 0:fd07992b1c32 | 634 | } |
piniels | 0:fd07992b1c32 | 635 | |
piniels | 0:fd07992b1c32 | 636 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 637 | //pump.goToZeroPosition(pumpSpeed); |
piniels | 0:fd07992b1c32 | 638 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 639 | //pump.goToZeroPosition(pumpSpeed); |
piniels | 0:fd07992b1c32 | 640 | //pump.step(1,1,1); |
piniels | 0:fd07992b1c32 | 641 | #endif |
piniels | 0:fd07992b1c32 | 642 | |
piniels | 0:fd07992b1c32 | 643 | } |
piniels | 0:fd07992b1c32 | 644 | //-------------------------------------------------------TYMP-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 645 | else if(recv_report.data[0] == 'T' && recv_report.data[1] == 'Y' && recv_report.data[2] == 'M' && recv_report.data[3] == 'P') { |
piniels | 0:fd07992b1c32 | 646 | pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 647 | minPressure = -1*(((recv_report.data[9] - 0x30) * 100) + ((recv_report.data[10] - 0x30) * 10) + ((recv_report.data[11] - 0x30))); |
piniels | 0:fd07992b1c32 | 648 | maxPressure = ((recv_report.data[13] - 0x30) * 100) + ((recv_report.data[14] - 0x30) * 10) + ((recv_report.data[15] - 0x30)); |
piniels | 0:fd07992b1c32 | 649 | minPressureFloat = convertDaPAToNominalFloat(minPressure); |
piniels | 0:fd07992b1c32 | 650 | maxPressureFloat = convertDaPAToNominalFloat(maxPressure); |
piniels | 0:fd07992b1c32 | 651 | |
piniels | 0:fd07992b1c32 | 652 | //True if DC motor |
piniels | 0:fd07992b1c32 | 653 | if(recv_report.data[17] == 'T' && recv_report.data[18] == 'r' && recv_report.data[19] == 'u' && recv_report.data[20] == 'e') { |
piniels | 0:fd07992b1c32 | 654 | dcMotor = true; |
piniels | 0:fd07992b1c32 | 655 | //float pumpspeedNorm = ((( (float) pumpSpeed-15)/6.75)/24); /// HACK for testing pump from WPM |
piniels | 0:fd07992b1c32 | 656 | controller.setBias(0.3333); |
piniels | 0:fd07992b1c32 | 657 | controller.setOutputLimits(-1, 1); |
piniels | 0:fd07992b1c32 | 658 | microSteps = (recv_report.data[22] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 659 | // pc.printf("DC Motor: %d",pumpspeedNorm); |
piniels | 0:fd07992b1c32 | 660 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 661 | } else { |
piniels | 0:fd07992b1c32 | 662 | dcMotor = false; |
piniels | 0:fd07992b1c32 | 663 | //RPM output (stepper motor) from -400.0 to 400.0 |
piniels | 0:fd07992b1c32 | 664 | controller.setBias(2.0); |
piniels | 0:fd07992b1c32 | 665 | controller.setOutputLimits(-pumpSpeed, pumpSpeed); |
piniels | 0:fd07992b1c32 | 666 | microSteps = (recv_report.data[23] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 667 | pc.printf("Stepper Motor, speed: %i minPressure: %i maxPressure %i minPressureFloat: %f maxPressureFloat %f", pumpSpeed, minPressure, maxPressure, minPressureFloat, maxPressureFloat); |
piniels | 0:fd07992b1c32 | 668 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 669 | |
piniels | 0:fd07992b1c32 | 670 | } |
piniels | 0:fd07992b1c32 | 671 | |
piniels | 0:fd07992b1c32 | 672 | |
piniels | 0:fd07992b1c32 | 673 | pc.printf("MicroSteps: %i", microSteps); |
piniels | 0:fd07992b1c32 | 674 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 675 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 676 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 677 | #endif |
piniels | 0:fd07992b1c32 | 678 | |
piniels | 0:fd07992b1c32 | 679 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 680 | controller.setSetPoint(minPressureFloat); |
piniels | 0:fd07992b1c32 | 681 | stateTympSweep = 1; |
piniels | 0:fd07992b1c32 | 682 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 683 | sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds) |
piniels | 0:fd07992b1c32 | 684 | startTymp = true; |
piniels | 0:fd07992b1c32 | 685 | startSweep = false; |
piniels | 0:fd07992b1c32 | 686 | startWear = false; |
piniels | 0:fd07992b1c32 | 687 | } |
piniels | 0:fd07992b1c32 | 688 | |
piniels | 0:fd07992b1c32 | 689 | //-------------------------------------------------------WEAR Leak test-----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 690 | else if(recv_report.data[0] == 'W' && recv_report.data[1] == 'L' && recv_report.data[2] == 'E' && recv_report.data[3] == 'A') { |
piniels | 0:fd07992b1c32 | 691 | pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 692 | |
piniels | 0:fd07992b1c32 | 693 | //check if negative pressure1is wanted |
piniels | 0:fd07992b1c32 | 694 | int negativevalue = 0; |
piniels | 0:fd07992b1c32 | 695 | if(recv_report.data[9] == '-') { |
piniels | 0:fd07992b1c32 | 696 | negativevalue = 1; |
piniels | 0:fd07992b1c32 | 697 | 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)); |
piniels | 0:fd07992b1c32 | 698 | } else { |
piniels | 0:fd07992b1c32 | 699 | setPointMessage = (((int)(recv_report.data[9]) - 0x30) * 100) + (((int)(recv_report.data[10]) - 0x30) * 10) + (((int)(recv_report.data[11]) - 0x30)); |
piniels | 0:fd07992b1c32 | 700 | } |
piniels | 0:fd07992b1c32 | 701 | |
piniels | 0:fd07992b1c32 | 702 | |
piniels | 0:fd07992b1c32 | 703 | setPointFloat = convertDaPAToNominalFloat(setPointMessage); |
piniels | 0:fd07992b1c32 | 704 | |
piniels | 0:fd07992b1c32 | 705 | dcMotor = false; |
piniels | 0:fd07992b1c32 | 706 | controller.setBias(2.0); |
piniels | 0:fd07992b1c32 | 707 | controller.setOutputLimits(-pumpSpeed, pumpSpeed); |
piniels | 0:fd07992b1c32 | 708 | microSteps = (recv_report.data[13 + negativevalue] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 709 | pc.printf("Wear leak test, speed: %i setPointMessage: %i ", pumpSpeed, setPointMessage); |
piniels | 0:fd07992b1c32 | 710 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 711 | |
piniels | 0:fd07992b1c32 | 712 | |
piniels | 0:fd07992b1c32 | 713 | |
piniels | 0:fd07992b1c32 | 714 | |
piniels | 0:fd07992b1c32 | 715 | pc.printf("MicroSteps: %i", microSteps); |
piniels | 0:fd07992b1c32 | 716 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 717 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 718 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 719 | #endif |
piniels | 0:fd07992b1c32 | 720 | |
piniels | 0:fd07992b1c32 | 721 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 722 | controller.setSetPoint(setPointFloat); |
piniels | 0:fd07992b1c32 | 723 | stateTympSweep = 4; |
piniels | 0:fd07992b1c32 | 724 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 725 | //sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds) |
piniels | 0:fd07992b1c32 | 726 | startWear = true; |
piniels | 0:fd07992b1c32 | 727 | startTymp = false; |
piniels | 0:fd07992b1c32 | 728 | startSweep = false; |
piniels | 0:fd07992b1c32 | 729 | } |
piniels | 0:fd07992b1c32 | 730 | //-------------------------------------------------------WEAR -----------------------------------------------------------------------------------// |
piniels | 0:fd07992b1c32 | 731 | else if(recv_report.data[0] == 'W' && recv_report.data[1] == 'E' && recv_report.data[2] == 'A' && recv_report.data[3] == 'R') { |
piniels | 0:fd07992b1c32 | 732 | pumpSpeed = (((int)(recv_report.data[5]) - 0x30) * 100) + (((int)(recv_report.data[6]) - 0x30) * 10) + (((int)(recv_report.data[7]) - 0x30)); |
piniels | 0:fd07992b1c32 | 733 | minPressure = -1*(((recv_report.data[9] - 0x30) * 100) + ((recv_report.data[10] - 0x30) * 10) + ((recv_report.data[11] - 0x30))); |
piniels | 0:fd07992b1c32 | 734 | maxPressure = ((recv_report.data[13] - 0x30) * 100) + ((recv_report.data[14] - 0x30) * 10) + ((recv_report.data[15] - 0x30)); |
piniels | 0:fd07992b1c32 | 735 | tPauseSec = ((recv_report.data[17] - 0x30) * 100) + ((recv_report.data[18] - 0x30) * 10) + ((recv_report.data[19] - 0x30)); |
piniels | 0:fd07992b1c32 | 736 | tTimeOutSec = ((recv_report.data[21] - 0x30) * 100) + ((recv_report.data[22] - 0x30) * 10) + ((recv_report.data[23] - 0x30)); |
piniels | 0:fd07992b1c32 | 737 | |
piniels | 0:fd07992b1c32 | 738 | minPressureFloat = convertDaPAToNominalFloat(minPressure); |
piniels | 0:fd07992b1c32 | 739 | maxPressureFloat = convertDaPAToNominalFloat(maxPressure); |
piniels | 0:fd07992b1c32 | 740 | |
piniels | 0:fd07992b1c32 | 741 | dcMotor = false; |
piniels | 0:fd07992b1c32 | 742 | controller.setBias(2.0); |
piniels | 0:fd07992b1c32 | 743 | controller.setOutputLimits(-pumpSpeed, pumpSpeed); |
piniels | 0:fd07992b1c32 | 744 | microSteps = (recv_report.data[25] - 0x30); //MicroSteps |
piniels | 0:fd07992b1c32 | 745 | pc.printf("Wear, speed: %i minPressure: %i maxPressure %i tPause: %i tTimeOut %i", pumpSpeed, minPressure, maxPressure, tPauseSec, tTimeOutSec); |
piniels | 0:fd07992b1c32 | 746 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 747 | |
piniels | 0:fd07992b1c32 | 748 | |
piniels | 0:fd07992b1c32 | 749 | |
piniels | 0:fd07992b1c32 | 750 | |
piniels | 0:fd07992b1c32 | 751 | pc.printf("MicroSteps: %i", microSteps); |
piniels | 0:fd07992b1c32 | 752 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 753 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 754 | pump.setMicroSteps(microSteps); |
piniels | 0:fd07992b1c32 | 755 | #endif |
piniels | 0:fd07992b1c32 | 756 | |
piniels | 0:fd07992b1c32 | 757 | pump.openCloseValve(false); |
piniels | 0:fd07992b1c32 | 758 | controller.setSetPoint(minPressureFloat); |
piniels | 0:fd07992b1c32 | 759 | stateTympSweep = 1; |
piniels | 0:fd07992b1c32 | 760 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 761 | //sampleTick.attach_us(&samplepressureSensor1, 1000); // the address of the function to be attached (sample of presure sensor) and the interval (1 milliseconds) |
piniels | 0:fd07992b1c32 | 762 | startWear = true; |
piniels | 0:fd07992b1c32 | 763 | startTymp = false; |
piniels | 0:fd07992b1c32 | 764 | startSweep = false; |
piniels | 0:fd07992b1c32 | 765 | } |
piniels | 0:fd07992b1c32 | 766 | |
piniels | 0:fd07992b1c32 | 767 | } |
piniels | 0:fd07992b1c32 | 768 | |
piniels | 0:fd07992b1c32 | 769 | /* Main loop */ |
piniels | 0:fd07992b1c32 | 770 | |
piniels | 0:fd07992b1c32 | 771 | int main() |
piniels | 0:fd07992b1c32 | 772 | { |
piniels | 0:fd07992b1c32 | 773 | |
piniels | 0:fd07992b1c32 | 774 | //Analog input from 0.0 to 3.3V |
piniels | 0:fd07992b1c32 | 775 | controller.setInputLimits(0.0, 1.0); //Todo: should this be 0.0,1.0? |
piniels | 0:fd07992b1c32 | 776 | //RPM output (stepper motor) from -300.0 to 300.0 |
piniels | 0:fd07992b1c32 | 777 | controller.setOutputLimits(-300.0, 300.0); |
piniels | 0:fd07992b1c32 | 778 | //If there's a bias. |
piniels | 0:fd07992b1c32 | 779 | controller.setBias(15.0); |
piniels | 0:fd07992b1c32 | 780 | controller.setMode(AUTO_MODE); |
piniels | 0:fd07992b1c32 | 781 | //We want the process variable to be 2.0V |
piniels | 0:fd07992b1c32 | 782 | controller.setSetPoint(0.6); |
piniels | 0:fd07992b1c32 | 783 | |
piniels | 0:fd07992b1c32 | 784 | //Setup debug port to 115200 baud |
piniels | 0:fd07992b1c32 | 785 | pc.baud(115200); |
piniels | 0:fd07992b1c32 | 786 | |
piniels | 0:fd07992b1c32 | 787 | //Report length on HID report |
piniels | 0:fd07992b1c32 | 788 | send_report.length = 63; |
piniels | 0:fd07992b1c32 | 789 | |
piniels | 0:fd07992b1c32 | 790 | //Init the motor control driver for SPI communication |
piniels | 0:fd07992b1c32 | 791 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 792 | //pump.initSpi(); |
piniels | 0:fd07992b1c32 | 793 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 794 | pump.initSpi(); |
piniels | 0:fd07992b1c32 | 795 | #endif |
piniels | 0:fd07992b1c32 | 796 | |
piniels | 0:fd07992b1c32 | 797 | //Setting up periode for RGB LED |
piniels | 0:fd07992b1c32 | 798 | red.period(0.001); |
piniels | 0:fd07992b1c32 | 799 | green.period(0.001); |
piniels | 0:fd07992b1c32 | 800 | blue.period(0.001); |
piniels | 0:fd07992b1c32 | 801 | |
piniels | 0:fd07992b1c32 | 802 | |
piniels | 0:fd07992b1c32 | 803 | |
piniels | 0:fd07992b1c32 | 804 | while(1) { |
piniels | 0:fd07992b1c32 | 805 | //try to read a msg |
piniels | 0:fd07992b1c32 | 806 | if(hid.readNB(&recv_report)) |
piniels | 0:fd07992b1c32 | 807 | { |
piniels | 0:fd07992b1c32 | 808 | parseIncommingMessage(); |
piniels | 0:fd07992b1c32 | 809 | } |
piniels | 0:fd07992b1c32 | 810 | |
piniels | 0:fd07992b1c32 | 811 | if(startSweep) |
piniels | 0:fd07992b1c32 | 812 | { |
piniels | 0:fd07992b1c32 | 813 | if(pressureFloat <= (minPressureFloat + 0.02 )) |
piniels | 0:fd07992b1c32 | 814 | { |
piniels | 0:fd07992b1c32 | 815 | controller.setSetPoint(maxPressureFloat); |
piniels | 0:fd07992b1c32 | 816 | } |
piniels | 0:fd07992b1c32 | 817 | else if(pressureFloat >= (maxPressureFloat - 0.02)) |
piniels | 0:fd07992b1c32 | 818 | { |
piniels | 0:fd07992b1c32 | 819 | controller.setSetPoint(minPressureFloat); |
piniels | 0:fd07992b1c32 | 820 | } |
piniels | 0:fd07992b1c32 | 821 | |
piniels | 0:fd07992b1c32 | 822 | } |
piniels | 0:fd07992b1c32 | 823 | else if(startTymp) |
piniels | 0:fd07992b1c32 | 824 | { |
piniels | 0:fd07992b1c32 | 825 | |
piniels | 0:fd07992b1c32 | 826 | switch(stateTympSweep) |
piniels | 0:fd07992b1c32 | 827 | { |
piniels | 0:fd07992b1c32 | 828 | case 1: |
piniels | 0:fd07992b1c32 | 829 | { |
piniels | 0:fd07992b1c32 | 830 | |
piniels | 0:fd07992b1c32 | 831 | if(pressureFloat <= (minPressureFloat)) |
piniels | 0:fd07992b1c32 | 832 | { |
piniels | 0:fd07992b1c32 | 833 | controlTick.detach(); |
piniels | 0:fd07992b1c32 | 834 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 835 | wait(0.5); |
piniels | 0:fd07992b1c32 | 836 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 837 | controller.setSetPoint(maxPressureFloat); |
piniels | 0:fd07992b1c32 | 838 | stateTympSweep = 2; |
piniels | 0:fd07992b1c32 | 839 | } |
piniels | 0:fd07992b1c32 | 840 | |
piniels | 0:fd07992b1c32 | 841 | break; |
piniels | 0:fd07992b1c32 | 842 | } // case 1 |
piniels | 0:fd07992b1c32 | 843 | case 2: |
piniels | 0:fd07992b1c32 | 844 | { |
piniels | 0:fd07992b1c32 | 845 | if(pressureFloat >= (maxPressureFloat)) |
piniels | 0:fd07992b1c32 | 846 | { |
piniels | 0:fd07992b1c32 | 847 | controlTick.detach(); |
piniels | 0:fd07992b1c32 | 848 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 849 | wait(0.5); |
piniels | 0:fd07992b1c32 | 850 | controller.setSetPoint(0.5); |
piniels | 0:fd07992b1c32 | 851 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 852 | stateTympSweep = 3; |
piniels | 0:fd07992b1c32 | 853 | } |
piniels | 0:fd07992b1c32 | 854 | |
piniels | 0:fd07992b1c32 | 855 | break; |
piniels | 0:fd07992b1c32 | 856 | } // case 2 |
piniels | 0:fd07992b1c32 | 857 | case 3: |
piniels | 0:fd07992b1c32 | 858 | { |
piniels | 0:fd07992b1c32 | 859 | if(pressureFloat >= 0.4995 && pressureFloat <= 0.5005) |
piniels | 0:fd07992b1c32 | 860 | { |
piniels | 0:fd07992b1c32 | 861 | stateTympSweep = 0; |
piniels | 0:fd07992b1c32 | 862 | stopAll(); |
piniels | 0:fd07992b1c32 | 863 | } |
piniels | 0:fd07992b1c32 | 864 | |
piniels | 0:fd07992b1c32 | 865 | break; |
piniels | 0:fd07992b1c32 | 866 | } // case 3 |
piniels | 0:fd07992b1c32 | 867 | case 4: |
piniels | 0:fd07992b1c32 | 868 | { |
piniels | 0:fd07992b1c32 | 869 | //pc.printf("LEAK state"); |
piniels | 0:fd07992b1c32 | 870 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 871 | |
piniels | 0:fd07992b1c32 | 872 | |
piniels | 0:fd07992b1c32 | 873 | if(pressureFloat >= (setPointFloat - 0.001) && pressureFloat <= (setPointFloat + 0.001)) |
piniels | 0:fd07992b1c32 | 874 | { |
piniels | 0:fd07992b1c32 | 875 | stateTympSweep = 0; |
piniels | 0:fd07992b1c32 | 876 | stopMotor(); |
piniels | 0:fd07992b1c32 | 877 | |
piniels | 0:fd07992b1c32 | 878 | } |
piniels | 0:fd07992b1c32 | 879 | |
piniels | 0:fd07992b1c32 | 880 | break; |
piniels | 0:fd07992b1c32 | 881 | } // case 4 |
piniels | 0:fd07992b1c32 | 882 | |
piniels | 0:fd07992b1c32 | 883 | } // switch stateTympSweep |
piniels | 0:fd07992b1c32 | 884 | |
piniels | 0:fd07992b1c32 | 885 | |
piniels | 0:fd07992b1c32 | 886 | |
piniels | 0:fd07992b1c32 | 887 | } |
piniels | 0:fd07992b1c32 | 888 | else if(startWear) |
piniels | 0:fd07992b1c32 | 889 | { |
piniels | 0:fd07992b1c32 | 890 | |
piniels | 0:fd07992b1c32 | 891 | switch(stateTympSweep) |
piniels | 0:fd07992b1c32 | 892 | { |
piniels | 0:fd07992b1c32 | 893 | case 1: |
piniels | 0:fd07992b1c32 | 894 | { |
piniels | 0:fd07992b1c32 | 895 | |
piniels | 0:fd07992b1c32 | 896 | if(pressureFloat <= (minPressureFloat + 0.01)) //tolerance on 20 daPA |
piniels | 0:fd07992b1c32 | 897 | { |
piniels | 0:fd07992b1c32 | 898 | controlTick.detach(); |
piniels | 0:fd07992b1c32 | 899 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 900 | wait(1); |
piniels | 0:fd07992b1c32 | 901 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 902 | controller.setSetPoint(maxPressureFloat); |
piniels | 0:fd07992b1c32 | 903 | stateTympSweep = 2; |
piniels | 0:fd07992b1c32 | 904 | } |
piniels | 0:fd07992b1c32 | 905 | |
piniels | 0:fd07992b1c32 | 906 | break; |
piniels | 0:fd07992b1c32 | 907 | } // case 1 |
piniels | 0:fd07992b1c32 | 908 | case 2: |
piniels | 0:fd07992b1c32 | 909 | { |
piniels | 0:fd07992b1c32 | 910 | if(pressureFloat >= (maxPressureFloat - 0.01)) //tolerance on 20 daPA |
piniels | 0:fd07992b1c32 | 911 | { |
piniels | 0:fd07992b1c32 | 912 | controlTick.detach(); |
piniels | 0:fd07992b1c32 | 913 | pump.runStepperAtSpeed(false,0,0); |
piniels | 0:fd07992b1c32 | 914 | wait(1); |
piniels | 0:fd07992b1c32 | 915 | controller.setSetPoint(0.5); |
piniels | 0:fd07992b1c32 | 916 | controlTick.attach(&computeHIDController, RATE); // the address of the function to be attached (HID controller) and the interval (100 milliseconds) |
piniels | 0:fd07992b1c32 | 917 | stateTympSweep = 3; |
piniels | 0:fd07992b1c32 | 918 | } |
piniels | 0:fd07992b1c32 | 919 | |
piniels | 0:fd07992b1c32 | 920 | break; |
piniels | 0:fd07992b1c32 | 921 | } // case 2 |
piniels | 0:fd07992b1c32 | 922 | case 3: |
piniels | 0:fd07992b1c32 | 923 | { |
piniels | 0:fd07992b1c32 | 924 | if(pressureFloat >= 0.49 && pressureFloat <= 0.51) //tolerance on 20 daPA |
piniels | 0:fd07992b1c32 | 925 | { |
piniels | 0:fd07992b1c32 | 926 | stateTympSweep = 0; |
piniels | 0:fd07992b1c32 | 927 | stopAll(); |
piniels | 0:fd07992b1c32 | 928 | pc.printf("wear is finished!"); |
piniels | 0:fd07992b1c32 | 929 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 930 | sendWearFinished(); |
piniels | 0:fd07992b1c32 | 931 | } |
piniels | 0:fd07992b1c32 | 932 | |
piniels | 0:fd07992b1c32 | 933 | break; |
piniels | 0:fd07992b1c32 | 934 | } // case 3 |
piniels | 0:fd07992b1c32 | 935 | case 4: |
piniels | 0:fd07992b1c32 | 936 | { |
piniels | 0:fd07992b1c32 | 937 | //pc.printf("LEAK state"); |
piniels | 0:fd07992b1c32 | 938 | //pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 939 | |
piniels | 0:fd07992b1c32 | 940 | |
piniels | 0:fd07992b1c32 | 941 | if(pressureFloat >= (setPointFloat) ) |
piniels | 0:fd07992b1c32 | 942 | { |
piniels | 0:fd07992b1c32 | 943 | stateTympSweep = 0; |
piniels | 0:fd07992b1c32 | 944 | pc.printf("wear leak test is finished! pressure: %f setpoint: %f",pressureFloat, setPointFloat ); |
piniels | 0:fd07992b1c32 | 945 | pc.printf("\r\n"); |
piniels | 0:fd07992b1c32 | 946 | //sendWearLeakFinished(); |
piniels | 0:fd07992b1c32 | 947 | stopMotor(); |
piniels | 0:fd07992b1c32 | 948 | |
piniels | 0:fd07992b1c32 | 949 | } |
piniels | 0:fd07992b1c32 | 950 | |
piniels | 0:fd07992b1c32 | 951 | break; |
piniels | 0:fd07992b1c32 | 952 | } // case 4 |
piniels | 0:fd07992b1c32 | 953 | |
piniels | 0:fd07992b1c32 | 954 | |
piniels | 0:fd07992b1c32 | 955 | } // switch stateTympSweep |
piniels | 0:fd07992b1c32 | 956 | |
piniels | 0:fd07992b1c32 | 957 | |
piniels | 0:fd07992b1c32 | 958 | |
piniels | 0:fd07992b1c32 | 959 | } |
piniels | 0:fd07992b1c32 | 960 | |
piniels | 0:fd07992b1c32 | 961 | #ifdef CENTER_POSITION |
piniels | 0:fd07992b1c32 | 962 | |
piniels | 0:fd07992b1c32 | 963 | |
piniels | 0:fd07992b1c32 | 964 | |
piniels | 0:fd07992b1c32 | 965 | if(pump.getStepCount() > MAX_STEPS || pump.getStepCount() < (-1*MAX_STEPS) ) |
piniels | 0:fd07992b1c32 | 966 | { |
piniels | 0:fd07992b1c32 | 967 | stopAll(); |
piniels | 0:fd07992b1c32 | 968 | if(pump.getStepCount() > 0) |
piniels | 0:fd07992b1c32 | 969 | { |
piniels | 0:fd07992b1c32 | 970 | pumpSpeed = -MAX_SPEED; |
piniels | 0:fd07992b1c32 | 971 | } |
piniels | 0:fd07992b1c32 | 972 | else |
piniels | 0:fd07992b1c32 | 973 | { |
piniels | 0:fd07992b1c32 | 974 | pumpSpeed = MAX_SPEED; |
piniels | 0:fd07992b1c32 | 975 | } |
piniels | 0:fd07992b1c32 | 976 | |
piniels | 0:fd07992b1c32 | 977 | //#ifdef A4980 |
piniels | 0:fd07992b1c32 | 978 | //pump.goToZeroPosition(pumpSpeed); |
piniels | 0:fd07992b1c32 | 979 | #ifdef A4980PWM |
piniels | 0:fd07992b1c32 | 980 | pump.goToZeroPosition(pumpSpeed); |
piniels | 0:fd07992b1c32 | 981 | #endif |
piniels | 0:fd07992b1c32 | 982 | } |
piniels | 0:fd07992b1c32 | 983 | #endif |
piniels | 0:fd07992b1c32 | 984 | |
piniels | 0:fd07992b1c32 | 985 | red = (pressureFloat > 0.505) ? 1.0 - pressureFloat : (blue = pressureFloat); |
piniels | 0:fd07992b1c32 | 986 | green = (pressureFloat < 0.495) ? 1.0 - pressureFloat : (blue = pressureFloat); |
piniels | 0:fd07992b1c32 | 987 | //blue = 1; |
piniels | 0:fd07992b1c32 | 988 | wait(0.02); |
piniels | 0:fd07992b1c32 | 989 | |
piniels | 0:fd07992b1c32 | 990 | } |
piniels | 0:fd07992b1c32 | 991 | } |
piniels | 0:fd07992b1c32 | 992 | |
piniels | 0:fd07992b1c32 | 993 |