Ilsoe Peter / Mbed 2 deprecated HIDTympDeviceWithPIDController

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?

UserRevisionLine numberNew 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