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@0:bb537a3c7f2a, 2015-11-24 (annotated)
- Committer:
- mitchpang
- Date:
- Tue Nov 24 09:36:53 2015 +0000
- Revision:
- 0:bb537a3c7f2a
- Child:
- 2:819397348cd0
first commit
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 | 0:bb537a3c7f2a | 3 | #include "TB6612FNG.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 | 0:bb537a3c7f2a | 8 | #define SOLENOID_ON_DELAY 0.001 |
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 | 0:bb537a3c7f2a | 13 | |
mitchpang | 0:bb537a3c7f2a | 14 | DigitalOut led1(LED1,0); //Used as 1pps out indicator |
mitchpang | 0:bb537a3c7f2a | 15 | Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY); |
mitchpang | 0:bb537a3c7f2a | 16 | Solenoid led3(LED3, LED_ON_DELAY, LED_OFF_DELAY); |
mitchpang | 0:bb537a3c7f2a | 17 | Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY); |
mitchpang | 0:bb537a3c7f2a | 18 | DigitalOut onePPS_out(p29); |
mitchpang | 0:bb537a3c7f2a | 19 | Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay) |
mitchpang | 0:bb537a3c7f2a | 20 | TB6612FNG augerMotors(p21, p26, p25, p22, p24, p23, p27); //TB6612FNG( PinName pinPwmA, PinName pinAin1, PinName pinAin2, PinName pinPwmB, PinName pinBin1, PinName pinBin2, PinName pinNStby ); |
mitchpang | 0:bb537a3c7f2a | 21 | AnalogIn topMotorAdjuster(p15); |
mitchpang | 0:bb537a3c7f2a | 22 | AnalogIn bottomMotorAdjuster(p16); |
mitchpang | 0:bb537a3c7f2a | 23 | DigitalOut unused1(p17); |
mitchpang | 0:bb537a3c7f2a | 24 | AnalogIn og1(p20); |
mitchpang | 0:bb537a3c7f2a | 25 | AnalogIn og2(p19); |
mitchpang | 0:bb537a3c7f2a | 26 | AnalogIn og3(p18); |
mitchpang | 0:bb537a3c7f2a | 27 | Timer totalT; |
mitchpang | 0:bb537a3c7f2a | 28 | //Ticker 1pps; |
mitchpang | 0:bb537a3c7f2a | 29 | |
mitchpang | 0:bb537a3c7f2a | 30 | Serial pc(USBTX,USBRX); //used for debugging |
mitchpang | 0:bb537a3c7f2a | 31 | |
mitchpang | 0:bb537a3c7f2a | 32 | float og1Threshold = 0; |
mitchpang | 0:bb537a3c7f2a | 33 | float og2Threshold = 0; |
mitchpang | 0:bb537a3c7f2a | 34 | float og3Threshold = 0; |
mitchpang | 0:bb537a3c7f2a | 35 | int og1Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 36 | int og2Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 37 | int og3Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 38 | float og1_adc = 0; |
mitchpang | 0:bb537a3c7f2a | 39 | float og2_adc = 0; |
mitchpang | 0:bb537a3c7f2a | 40 | float og3_adc = 0; |
mitchpang | 0:bb537a3c7f2a | 41 | int og1Ndx = 0; |
mitchpang | 0:bb537a3c7f2a | 42 | int og2Ndx = 0; |
mitchpang | 0:bb537a3c7f2a | 43 | int og3Ndx = 0; |
mitchpang | 0:bb537a3c7f2a | 44 | float og1_min = 0; |
mitchpang | 0:bb537a3c7f2a | 45 | float og1_max = 0; |
mitchpang | 0:bb537a3c7f2a | 46 | float og2_min = 0; |
mitchpang | 0:bb537a3c7f2a | 47 | float og2_max = 0; |
mitchpang | 0:bb537a3c7f2a | 48 | float og3_min = 0; |
mitchpang | 0:bb537a3c7f2a | 49 | float og3_max = 0; |
mitchpang | 0:bb537a3c7f2a | 50 | |
mitchpang | 0:bb537a3c7f2a | 51 | Regrind regrindArray[REGRIND_ARRAY_SIZE]; |
mitchpang | 0:bb537a3c7f2a | 52 | /* |
mitchpang | 0:bb537a3c7f2a | 53 | void flip(){ |
mitchpang | 0:bb537a3c7f2a | 54 | led1 = !led1; |
mitchpang | 0:bb537a3c7f2a | 55 | 1pps_out = !1pps_out; |
mitchpang | 0:bb537a3c7f2a | 56 | } |
mitchpang | 0:bb537a3c7f2a | 57 | */ |
mitchpang | 0:bb537a3c7f2a | 58 | |
mitchpang | 0:bb537a3c7f2a | 59 | int main() { |
mitchpang | 0:bb537a3c7f2a | 60 | //Start Clock |
mitchpang | 0:bb537a3c7f2a | 61 | totalT.start(); |
mitchpang | 0:bb537a3c7f2a | 62 | //Setup Information |
mitchpang | 0:bb537a3c7f2a | 63 | /* |
mitchpang | 0:bb537a3c7f2a | 64 | led1 = 0; |
mitchpang | 0:bb537a3c7f2a | 65 | 1pps_out = 0; |
mitchpang | 0:bb537a3c7f2a | 66 | 1pps.attach(&flip, 1.0); |
mitchpang | 0:bb537a3c7f2a | 67 | */ |
mitchpang | 0:bb537a3c7f2a | 68 | //Setup motors |
mitchpang | 0:bb537a3c7f2a | 69 | augerMotors.setPwmApulsewidth(0.0); |
mitchpang | 0:bb537a3c7f2a | 70 | augerMotors.setPwmBpulsewidth(0.0); |
mitchpang | 0:bb537a3c7f2a | 71 | augerMotors.motorA_ccw(); |
mitchpang | 0:bb537a3c7f2a | 72 | augerMotors.motorB_ccw(); |
mitchpang | 0:bb537a3c7f2a | 73 | |
mitchpang | 0:bb537a3c7f2a | 74 | //Spin up Motors until fluctuation of 10% seen |
mitchpang | 0:bb537a3c7f2a | 75 | |
mitchpang | 0:bb537a3c7f2a | 76 | |
mitchpang | 0:bb537a3c7f2a | 77 | //Calibrate the ADC |
mitchpang | 0:bb537a3c7f2a | 78 | //Done by spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min |
mitchpang | 0:bb537a3c7f2a | 79 | |
mitchpang | 0:bb537a3c7f2a | 80 | while(1) { |
mitchpang | 0:bb537a3c7f2a | 81 | //Sample ADCs |
mitchpang | 0:bb537a3c7f2a | 82 | og1_adc = og1; |
mitchpang | 0:bb537a3c7f2a | 83 | og2_adc = og2; |
mitchpang | 0:bb537a3c7f2a | 84 | og3_adc = og3; |
mitchpang | 0:bb537a3c7f2a | 85 | |
mitchpang | 0:bb537a3c7f2a | 86 | if((og1_adc < og1Threshold) && (og1Oneshot != 1)){ //Something passed through og1 |
mitchpang | 0:bb537a3c7f2a | 87 | og1Oneshot = 1; |
mitchpang | 0:bb537a3c7f2a | 88 | pc.printf("Regrind seen at OG 1 : %dV\n\r", 3.3*og1_adc); |
mitchpang | 0:bb537a3c7f2a | 89 | led2 = 1; |
mitchpang | 0:bb537a3c7f2a | 90 | //Create Regrind |
mitchpang | 0:bb537a3c7f2a | 91 | regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0); |
mitchpang | 0:bb537a3c7f2a | 92 | |
mitchpang | 0:bb537a3c7f2a | 93 | } //if(og1...) |
mitchpang | 0:bb537a3c7f2a | 94 | else if(og1_adc > 0.95*og1_max){ //Regrind has passed ok to reset og |
mitchpang | 0:bb537a3c7f2a | 95 | og1Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 96 | |
mitchpang | 0:bb537a3c7f2a | 97 | }//else if(og1 ...) |
mitchpang | 0:bb537a3c7f2a | 98 | |
mitchpang | 0:bb537a3c7f2a | 99 | if((og2_adc < og2Threshold) && (og2Oneshot != 1)){ |
mitchpang | 0:bb537a3c7f2a | 100 | og2Oneshot = 1; |
mitchpang | 0:bb537a3c7f2a | 101 | regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST); |
mitchpang | 0:bb537a3c7f2a | 102 | led3 = 1; |
mitchpang | 0:bb537a3c7f2a | 103 | }//if(og2..) |
mitchpang | 0:bb537a3c7f2a | 104 | else if(og2_adc > 0.95*og2_max){ //Regrind has passed ok to reset og |
mitchpang | 0:bb537a3c7f2a | 105 | og2Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 106 | |
mitchpang | 0:bb537a3c7f2a | 107 | }//else if(og2 ...) |
mitchpang | 0:bb537a3c7f2a | 108 | |
mitchpang | 0:bb537a3c7f2a | 109 | if((og3_adc < og3Threshold) && (og3Oneshot != 1)){ |
mitchpang | 0:bb537a3c7f2a | 110 | og3Oneshot = 1; |
mitchpang | 0:bb537a3c7f2a | 111 | led4 = 1; |
mitchpang | 0:bb537a3c7f2a | 112 | regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST); |
mitchpang | 0:bb537a3c7f2a | 113 | if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid. |
mitchpang | 0:bb537a3c7f2a | 114 | solenoid = 1; |
mitchpang | 0:bb537a3c7f2a | 115 | } |
mitchpang | 0:bb537a3c7f2a | 116 | }//if(og3..) |
mitchpang | 0:bb537a3c7f2a | 117 | else if(og3_adc > 0.95*og3_max){ //Regrind has passed ok to reset og |
mitchpang | 0:bb537a3c7f2a | 118 | og3Oneshot = 0; |
mitchpang | 0:bb537a3c7f2a | 119 | |
mitchpang | 0:bb537a3c7f2a | 120 | }//else if(og3 ...) |
mitchpang | 0:bb537a3c7f2a | 121 | |
mitchpang | 0:bb537a3c7f2a | 122 | //Check on 1pps clock |
mitchpang | 0:bb537a3c7f2a | 123 | if((totalT.read_us() % 2000000) < 1000000){ |
mitchpang | 0:bb537a3c7f2a | 124 | led1 = 1; |
mitchpang | 0:bb537a3c7f2a | 125 | onePPS_out = 1; |
mitchpang | 0:bb537a3c7f2a | 126 | } |
mitchpang | 0:bb537a3c7f2a | 127 | else {//timer is in off cycle |
mitchpang | 0:bb537a3c7f2a | 128 | led1 = 0; |
mitchpang | 0:bb537a3c7f2a | 129 | onePPS_out = 0; |
mitchpang | 0:bb537a3c7f2a | 130 | } |
mitchpang | 0:bb537a3c7f2a | 131 | |
mitchpang | 0:bb537a3c7f2a | 132 | //Check if data writing flag is set - if so, write to SD card |
mitchpang | 0:bb537a3c7f2a | 133 | |
mitchpang | 0:bb537a3c7f2a | 134 | //Adjust PWM as necessary |
mitchpang | 0:bb537a3c7f2a | 135 | augerMotors.setPwmApulsewidth(topMotorAdjuster.read()); |
mitchpang | 0:bb537a3c7f2a | 136 | augerMotors.setPwmBpulsewidth(bottomMotorAdjuster.read()); |
mitchpang | 0:bb537a3c7f2a | 137 | } //while(1) |
mitchpang | 0:bb537a3c7f2a | 138 | totalT.stop(); |
mitchpang | 0:bb537a3c7f2a | 139 | }// int main() |