Fork of expo day code. Fires on every detected particle to calibrate delay time to solenoid firing, and solenoid on time.
Dependencies: Regrind RioRandHBridge Solenoid mbed
Fork of ECE4012-PHD by
main.cpp@5:be4b501b9373, 2015-12-09 (annotated)
- Committer:
- mitchpang
- Date:
- Wed Dec 09 19:55:27 2015 +0000
- Revision:
- 5:be4b501b9373
- Parent:
- 3:597f8492fee2
Fork of expo day code. Changed logic to actuate on all regrinds to test accuracy of system and find calibration of delay to solenoid firing and how long it fires for.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
mitchpang | 0:bb537a3c7f2a | 1 | #include "mbed.h" |
mitchpang | 0:bb537a3c7f2a | 2 | #include "Regrind.h" |
mitchpang | 3:597f8492fee2 | 3 | #include "RioRandHBridge.h" |
mitchpang | 0:bb537a3c7f2a | 4 | #include "Solenoid.h" |
mitchpang | 0:bb537a3c7f2a | 5 | |
mitchpang | 0:bb537a3c7f2a | 6 | #define OG1_TO_OG2_DIST 1 |
mitchpang | 0:bb537a3c7f2a | 7 | #define OG1_TO_OG3_DIST 2 |
mitchpang | 5:be4b501b9373 | 8 | #define SOLENOID_ON_DELAY 0.25 |
mitchpang | 0:bb537a3c7f2a | 9 | #define SOLENOID_OFF_DELAY 0.0 |
mitchpang | 0:bb537a3c7f2a | 10 | #define LED_ON_DELAY 0.5 |
mitchpang | 0:bb537a3c7f2a | 11 | #define LED_OFF_DELAY 0.0 |
mitchpang | 0:bb537a3c7f2a | 12 | #define REGRIND_ARRAY_SIZE 1 |
mitchpang | 5:be4b501b9373 | 13 | #define THRESHOLD 0.02 |
mitchpang | 2:819397348cd0 | 14 | #define RETURN_THRESHOLD 0.01 |
mitchpang | 0:bb537a3c7f2a | 15 | |
mitchpang | 5:be4b501b9373 | 16 | Solenoid led1(LED1, LED_ON_DELAY, LED_OFF_DELAY); //Used as 1pps out indicator |
mitchpang | 0:bb537a3c7f2a | 17 | Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY); |
mitchpang | 0:bb537a3c7f2a | 18 | Solenoid led3(LED3, LED_ON_DELAY, LED_OFF_DELAY); |
mitchpang | 0:bb537a3c7f2a | 19 | Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY); |
mitchpang | 0:bb537a3c7f2a | 20 | DigitalOut onePPS_out(p29); |
mitchpang | 0:bb537a3c7f2a | 21 | Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay) |
mitchpang | 3:597f8492fee2 | 22 | RioRandHBridge augerMotors(p21, p25, p22, p23); //RioRandHBridge( PinName pinPwm1, PinName pinDir1, PinName pinPwm2, PinName pinDir2); |
mitchpang | 3:597f8492fee2 | 23 | DigitalIn reverseMotor1pb(p26); |
mitchpang | 3:597f8492fee2 | 24 | DigitalIn reverseMotor2pb(p24); |
mitchpang | 2:819397348cd0 | 25 | AnalogIn topMotorAdjuster(p19); |
mitchpang | 3:597f8492fee2 | 26 | DigitalOut bottomMotorAdjuster(p16,0); |
mitchpang | 2:819397348cd0 | 27 | DigitalOut unused1(p17,0); |
mitchpang | 5:be4b501b9373 | 28 | AnalogIn og1(p16); |
mitchpang | 2:819397348cd0 | 29 | AnalogIn og2(p20); |
mitchpang | 3:597f8492fee2 | 30 | DigitalOut og3(p18,0); |
mitchpang | 2:819397348cd0 | 31 | InterruptIn divertParticle(p5); |
mitchpang | 0:bb537a3c7f2a | 32 | Timer totalT; |
mitchpang | 2:819397348cd0 | 33 | DigitalOut startColor(p6); |
mitchpang | 2:819397348cd0 | 34 | //DigitalIn redColor(p5); |
mitchpang | 2:819397348cd0 | 35 | DigitalIn blueColor(p7); |
mitchpang | 2:819397348cd0 | 36 | DigitalIn greenColor(p8); |
mitchpang | 0:bb537a3c7f2a | 37 | //Ticker 1pps; |
mitchpang | 0:bb537a3c7f2a | 38 | |
mitchpang | 0:bb537a3c7f2a | 39 | Serial pc(USBTX,USBRX); //used for debugging |
mitchpang | 0:bb537a3c7f2a | 40 | |
mitchpang | 2:819397348cd0 | 41 | float og1Threshold = 0.3; |
mitchpang | 0:bb537a3c7f2a | 42 | float og2Threshold = 0; |
mitchpang | 0:bb537a3c7f2a | 43 | float og3Threshold = 0; |
mitchpang | 0:bb537a3c7f2a | 44 | int og1Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 45 | int og2Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 46 | int og3Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 47 | float og1_adc = 0; |
mitchpang | 0:bb537a3c7f2a | 48 | float og2_adc = 0; |
mitchpang | 0:bb537a3c7f2a | 49 | float og3_adc = 0; |
mitchpang | 0:bb537a3c7f2a | 50 | int og1Ndx = 0; |
mitchpang | 0:bb537a3c7f2a | 51 | int og2Ndx = 0; |
mitchpang | 0:bb537a3c7f2a | 52 | int og3Ndx = 0; |
mitchpang | 0:bb537a3c7f2a | 53 | float og1_min = 0; |
mitchpang | 2:819397348cd0 | 54 | float og1_max = 2.7; |
mitchpang | 0:bb537a3c7f2a | 55 | float og2_min = 0; |
mitchpang | 0:bb537a3c7f2a | 56 | float og2_max = 0; |
mitchpang | 0:bb537a3c7f2a | 57 | float og3_min = 0; |
mitchpang | 0:bb537a3c7f2a | 58 | float og3_max = 0; |
mitchpang | 2:819397348cd0 | 59 | float og1_calibration = 0; |
mitchpang | 2:819397348cd0 | 60 | float og2_calibration = 0; |
mitchpang | 2:819397348cd0 | 61 | float og3_calibration = 0; |
mitchpang | 0:bb537a3c7f2a | 62 | |
mitchpang | 0:bb537a3c7f2a | 63 | Regrind regrindArray[REGRIND_ARRAY_SIZE]; |
mitchpang | 0:bb537a3c7f2a | 64 | /* |
mitchpang | 0:bb537a3c7f2a | 65 | void flip(){ |
mitchpang | 0:bb537a3c7f2a | 66 | led1 = !led1; |
mitchpang | 0:bb537a3c7f2a | 67 | 1pps_out = !1pps_out; |
mitchpang | 0:bb537a3c7f2a | 68 | } |
mitchpang | 0:bb537a3c7f2a | 69 | */ |
mitchpang | 2:819397348cd0 | 70 | void divert(){ |
mitchpang | 2:819397348cd0 | 71 | regrindArray[og2Ndx].divert = 1; |
mitchpang | 5:be4b501b9373 | 72 | led3 = 1; |
mitchpang | 2:819397348cd0 | 73 | } |
mitchpang | 3:597f8492fee2 | 74 | void reverseMotor1(){ |
mitchpang | 3:597f8492fee2 | 75 | augerMotors.Dir1 = !augerMotors.Dir1; |
mitchpang | 3:597f8492fee2 | 76 | } |
mitchpang | 3:597f8492fee2 | 77 | void reverseMotor2(){ |
mitchpang | 3:597f8492fee2 | 78 | augerMotors.Dir2 = !augerMotors.Dir2; |
mitchpang | 3:597f8492fee2 | 79 | } |
mitchpang | 0:bb537a3c7f2a | 80 | |
mitchpang | 0:bb537a3c7f2a | 81 | int main() { |
mitchpang | 0:bb537a3c7f2a | 82 | //Start Clock |
mitchpang | 0:bb537a3c7f2a | 83 | totalT.start(); |
mitchpang | 0:bb537a3c7f2a | 84 | //Setup Information |
mitchpang | 0:bb537a3c7f2a | 85 | /* |
mitchpang | 0:bb537a3c7f2a | 86 | led1 = 0; |
mitchpang | 0:bb537a3c7f2a | 87 | 1pps_out = 0; |
mitchpang | 0:bb537a3c7f2a | 88 | 1pps.attach(&flip, 1.0); |
mitchpang | 0:bb537a3c7f2a | 89 | */ |
mitchpang | 0:bb537a3c7f2a | 90 | //Setup motors |
mitchpang | 3:597f8492fee2 | 91 | reverseMotor1pb.mode( PullUp ); |
mitchpang | 3:597f8492fee2 | 92 | reverseMotor2pb.mode( PullUp ); |
mitchpang | 3:597f8492fee2 | 93 | //reverseMotor1pb.attach_asserted( &reverseMotor1 ); |
mitchpang | 3:597f8492fee2 | 94 | //reverseMotor2pb.attach_asserted (&reverseMotor2); |
mitchpang | 3:597f8492fee2 | 95 | augerMotors.setpwm1pulsewidth(0.0); |
mitchpang | 3:597f8492fee2 | 96 | augerMotors.setpwm2pulsewidth(0.0); |
mitchpang | 3:597f8492fee2 | 97 | augerMotors.motor1_ccw(); |
mitchpang | 3:597f8492fee2 | 98 | augerMotors.motor2_ccw(); |
mitchpang | 0:bb537a3c7f2a | 99 | |
mitchpang | 0:bb537a3c7f2a | 100 | //Spin up Motors until fluctuation of 10% seen |
mitchpang | 0:bb537a3c7f2a | 101 | |
mitchpang | 0:bb537a3c7f2a | 102 | |
mitchpang | 0:bb537a3c7f2a | 103 | //Calibrate the ADC |
mitchpang | 0:bb537a3c7f2a | 104 | //Done by spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min |
mitchpang | 2:819397348cd0 | 105 | for(int i = 0; i<100;++i){ |
mitchpang | 2:819397348cd0 | 106 | og1_calibration += og1*3.3; |
mitchpang | 2:819397348cd0 | 107 | og2_calibration += og2*3.3; |
mitchpang | 2:819397348cd0 | 108 | } |
mitchpang | 2:819397348cd0 | 109 | og1_calibration = og1_calibration/100; |
mitchpang | 2:819397348cd0 | 110 | og2_calibration = og2_calibration/100; |
mitchpang | 2:819397348cd0 | 111 | printf("og1_calibration value: %f\n\r",og1_calibration); |
mitchpang | 2:819397348cd0 | 112 | printf("og2_calibration value: %f\n\r",og2_calibration); |
mitchpang | 2:819397348cd0 | 113 | wait(3); |
mitchpang | 2:819397348cd0 | 114 | |
mitchpang | 2:819397348cd0 | 115 | divertParticle.rise(&divert); |
mitchpang | 0:bb537a3c7f2a | 116 | |
mitchpang | 0:bb537a3c7f2a | 117 | while(1) { |
mitchpang | 2:819397348cd0 | 118 | /* |
mitchpang | 2:819397348cd0 | 119 | if(redColor == 1){ |
mitchpang | 2:819397348cd0 | 120 | solenoid = 1; //actuate solenoid if red |
mitchpang | 2:819397348cd0 | 121 | led1=1; |
mitchpang | 2:819397348cd0 | 122 | } |
mitchpang | 2:819397348cd0 | 123 | else { |
mitchpang | 2:819397348cd0 | 124 | solenoid = 0; //actuate solenoid no matter what. |
mitchpang | 2:819397348cd0 | 125 | led1=0; |
mitchpang | 2:819397348cd0 | 126 | } |
mitchpang | 2:819397348cd0 | 127 | */ |
mitchpang | 0:bb537a3c7f2a | 128 | //Sample ADCs |
mitchpang | 2:819397348cd0 | 129 | og1_adc = og1.read()*3.3; |
mitchpang | 2:819397348cd0 | 130 | og2_adc = og2.read()*3.3; |
mitchpang | 3:597f8492fee2 | 131 | //og3_adc = og3.read()*3.3; |
mitchpang | 2:819397348cd0 | 132 | //wait(0.01); |
mitchpang | 5:be4b501b9373 | 133 | //pc.printf("og1: %f og2: %f \n\r",og1_adc, og2_adc); |
mitchpang | 5:be4b501b9373 | 134 | |
mitchpang | 2:819397348cd0 | 135 | if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1 |
mitchpang | 0:bb537a3c7f2a | 136 | og1Oneshot = 1; |
mitchpang | 3:597f8492fee2 | 137 | divertParticle.rise(&divert); |
mitchpang | 2:819397348cd0 | 138 | //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc); |
mitchpang | 5:be4b501b9373 | 139 | led1 = 1; |
mitchpang | 2:819397348cd0 | 140 | |
mitchpang | 0:bb537a3c7f2a | 141 | //Create Regrind |
mitchpang | 0:bb537a3c7f2a | 142 | regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0); |
mitchpang | 2:819397348cd0 | 143 | startColor = 1; |
mitchpang | 2:819397348cd0 | 144 | //wait(1); |
mitchpang | 0:bb537a3c7f2a | 145 | } //if(og1...) |
mitchpang | 2:819397348cd0 | 146 | else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og |
mitchpang | 0:bb537a3c7f2a | 147 | og1Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 148 | |
mitchpang | 0:bb537a3c7f2a | 149 | }//else if(og1 ...) |
mitchpang | 0:bb537a3c7f2a | 150 | |
mitchpang | 2:819397348cd0 | 151 | if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){ |
mitchpang | 0:bb537a3c7f2a | 152 | og2Oneshot = 1; |
mitchpang | 3:597f8492fee2 | 153 | divertParticle.rise(NULL); |
mitchpang | 0:bb537a3c7f2a | 154 | regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST); |
mitchpang | 5:be4b501b9373 | 155 | led2 = 1; |
mitchpang | 2:819397348cd0 | 156 | if(regrindArray[og2Ndx].divert == 1){ |
mitchpang | 5:be4b501b9373 | 157 | wait_ms(90); |
mitchpang | 2:819397348cd0 | 158 | solenoid = 1; //actuate solenoid if red |
mitchpang | 5:be4b501b9373 | 159 | led4 = 1; |
mitchpang | 2:819397348cd0 | 160 | } |
mitchpang | 2:819397348cd0 | 161 | else { |
mitchpang | 5:be4b501b9373 | 162 | //wait_ms(90); |
mitchpang | 5:be4b501b9373 | 163 | solenoid = 1; //Dont actuate if not red. |
mitchpang | 5:be4b501b9373 | 164 | led4 = 1; |
mitchpang | 2:819397348cd0 | 165 | } |
mitchpang | 5:be4b501b9373 | 166 | regrindArray[og2Ndx].divert = 0; //reset divert flag |
mitchpang | 0:bb537a3c7f2a | 167 | }//if(og2..) |
mitchpang | 2:819397348cd0 | 168 | else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og |
mitchpang | 0:bb537a3c7f2a | 169 | og2Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 170 | |
mitchpang | 0:bb537a3c7f2a | 171 | }//else if(og2 ...) |
mitchpang | 3:597f8492fee2 | 172 | /* |
mitchpang | 2:819397348cd0 | 173 | if((og3_adc == 0) && (og3Oneshot != 1)){ |
mitchpang | 0:bb537a3c7f2a | 174 | og3Oneshot = 1; |
mitchpang | 0:bb537a3c7f2a | 175 | led4 = 1; |
mitchpang | 0:bb537a3c7f2a | 176 | regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST); |
mitchpang | 0:bb537a3c7f2a | 177 | if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid. |
mitchpang | 0:bb537a3c7f2a | 178 | solenoid = 1; |
mitchpang | 0:bb537a3c7f2a | 179 | } |
mitchpang | 0:bb537a3c7f2a | 180 | }//if(og3..) |
mitchpang | 2:819397348cd0 | 181 | else if(og3_adc == 1){ //Regrind has passed ok to reset og |
mitchpang | 0:bb537a3c7f2a | 182 | og3Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 183 | |
mitchpang | 0:bb537a3c7f2a | 184 | }//else if(og3 ...) |
mitchpang | 3:597f8492fee2 | 185 | */ |
mitchpang | 5:be4b501b9373 | 186 | /* |
mitchpang | 0:bb537a3c7f2a | 187 | //Check on 1pps clock |
mitchpang | 0:bb537a3c7f2a | 188 | if((totalT.read_us() % 2000000) < 1000000){ |
mitchpang | 0:bb537a3c7f2a | 189 | led1 = 1; |
mitchpang | 0:bb537a3c7f2a | 190 | onePPS_out = 1; |
mitchpang | 0:bb537a3c7f2a | 191 | } |
mitchpang | 0:bb537a3c7f2a | 192 | else {//timer is in off cycle |
mitchpang | 0:bb537a3c7f2a | 193 | led1 = 0; |
mitchpang | 0:bb537a3c7f2a | 194 | onePPS_out = 0; |
mitchpang | 5:be4b501b9373 | 195 | }*/ |
mitchpang | 0:bb537a3c7f2a | 196 | |
mitchpang | 0:bb537a3c7f2a | 197 | //Check if data writing flag is set - if so, write to SD card |
mitchpang | 0:bb537a3c7f2a | 198 | |
mitchpang | 0:bb537a3c7f2a | 199 | //Adjust PWM as necessary |
mitchpang | 3:597f8492fee2 | 200 | augerMotors.Dir1 = reverseMotor1pb; |
mitchpang | 3:597f8492fee2 | 201 | augerMotors.setpwm1pulsewidth(topMotorAdjuster.read()); |
mitchpang | 3:597f8492fee2 | 202 | augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read()); |
mitchpang | 2:819397348cd0 | 203 | //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read()); |
mitchpang | 0:bb537a3c7f2a | 204 | } //while(1) |
mitchpang | 0:bb537a3c7f2a | 205 | totalT.stop(); |
mitchpang | 0:bb537a3c7f2a | 206 | }// int main() |