Team Plastics Separation / Mbed 2 deprecated ECE4012-PHD_actuate_all

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 Mitchell Pang

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?

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