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@3:597f8492fee2, 2015-12-03 (annotated)

Committer:
mitchpang
Date:
Thu Dec 03 06:36:21 2015 +0000
Revision:
3:597f8492fee2
Parent:
2:819397348cd0
Child:
5:be4b501b9373
Changed H-Bridge

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 2:819397348cd0 8 #define SOLENOID_ON_DELAY 0.5
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 2:819397348cd0 13 #define THRESHOLD 0.015
mitchpang 2:819397348cd0 14 #define RETURN_THRESHOLD 0.01
mitchpang 0:bb537a3c7f2a 15
mitchpang 0:bb537a3c7f2a 16 DigitalOut led1(LED1,0); //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 2:819397348cd0 28 AnalogIn og1(p15);
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 2:819397348cd0 72 }
mitchpang 3:597f8492fee2 73 void reverseMotor1(){
mitchpang 3:597f8492fee2 74 augerMotors.Dir1 = !augerMotors.Dir1;
mitchpang 3:597f8492fee2 75 }
mitchpang 3:597f8492fee2 76 void reverseMotor2(){
mitchpang 3:597f8492fee2 77 augerMotors.Dir2 = !augerMotors.Dir2;
mitchpang 3:597f8492fee2 78 }
mitchpang 0:bb537a3c7f2a 79
mitchpang 0:bb537a3c7f2a 80 int main() {
mitchpang 0:bb537a3c7f2a 81 //Start Clock
mitchpang 0:bb537a3c7f2a 82 totalT.start();
mitchpang 0:bb537a3c7f2a 83 //Setup Information
mitchpang 0:bb537a3c7f2a 84 /*
mitchpang 0:bb537a3c7f2a 85 led1 = 0;
mitchpang 0:bb537a3c7f2a 86 1pps_out = 0;
mitchpang 0:bb537a3c7f2a 87 1pps.attach(&flip, 1.0);
mitchpang 0:bb537a3c7f2a 88 */
mitchpang 0:bb537a3c7f2a 89 //Setup motors
mitchpang 3:597f8492fee2 90 reverseMotor1pb.mode( PullUp );
mitchpang 3:597f8492fee2 91 reverseMotor2pb.mode( PullUp );
mitchpang 3:597f8492fee2 92 //reverseMotor1pb.attach_asserted( &reverseMotor1 );
mitchpang 3:597f8492fee2 93 //reverseMotor2pb.attach_asserted (&reverseMotor2);
mitchpang 3:597f8492fee2 94 augerMotors.setpwm1pulsewidth(0.0);
mitchpang 3:597f8492fee2 95 augerMotors.setpwm2pulsewidth(0.0);
mitchpang 3:597f8492fee2 96 augerMotors.motor1_ccw();
mitchpang 3:597f8492fee2 97 augerMotors.motor2_ccw();
mitchpang 0:bb537a3c7f2a 98
mitchpang 0:bb537a3c7f2a 99 //Spin up Motors until fluctuation of 10% seen
mitchpang 0:bb537a3c7f2a 100
mitchpang 0:bb537a3c7f2a 101
mitchpang 0:bb537a3c7f2a 102 //Calibrate the ADC
mitchpang 0:bb537a3c7f2a 103 //Done by spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min
mitchpang 2:819397348cd0 104 for(int i = 0; i<100;++i){
mitchpang 2:819397348cd0 105 og1_calibration += og1*3.3;
mitchpang 2:819397348cd0 106 og2_calibration += og2*3.3;
mitchpang 2:819397348cd0 107 }
mitchpang 2:819397348cd0 108 og1_calibration = og1_calibration/100;
mitchpang 2:819397348cd0 109 og2_calibration = og2_calibration/100;
mitchpang 2:819397348cd0 110 printf("og1_calibration value: %f\n\r",og1_calibration);
mitchpang 2:819397348cd0 111 printf("og2_calibration value: %f\n\r",og2_calibration);
mitchpang 2:819397348cd0 112 wait(3);
mitchpang 2:819397348cd0 113
mitchpang 2:819397348cd0 114 divertParticle.rise(&divert);
mitchpang 0:bb537a3c7f2a 115
mitchpang 0:bb537a3c7f2a 116 while(1) {
mitchpang 2:819397348cd0 117 /*
mitchpang 2:819397348cd0 118 if(redColor == 1){
mitchpang 2:819397348cd0 119 solenoid = 1; //actuate solenoid if red
mitchpang 2:819397348cd0 120 led1=1;
mitchpang 2:819397348cd0 121 }
mitchpang 2:819397348cd0 122 else {
mitchpang 2:819397348cd0 123 solenoid = 0; //actuate solenoid no matter what.
mitchpang 2:819397348cd0 124 led1=0;
mitchpang 2:819397348cd0 125 }
mitchpang 2:819397348cd0 126 */
mitchpang 0:bb537a3c7f2a 127 //Sample ADCs
mitchpang 2:819397348cd0 128 og1_adc = og1.read()*3.3;
mitchpang 2:819397348cd0 129 og2_adc = og2.read()*3.3;
mitchpang 3:597f8492fee2 130 //og3_adc = og3.read()*3.3;
mitchpang 2:819397348cd0 131 //wait(0.01);
mitchpang 2:819397348cd0 132 //pc.printf("og1: %f og2: %f og3: %f\n\r",og1_adc, og2_adc, og3_adc);
mitchpang 0:bb537a3c7f2a 133
mitchpang 2:819397348cd0 134 if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
mitchpang 0:bb537a3c7f2a 135 og1Oneshot = 1;
mitchpang 3:597f8492fee2 136 divertParticle.rise(&divert);
mitchpang 2:819397348cd0 137 //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
mitchpang 0:bb537a3c7f2a 138 led2 = 1;
mitchpang 2:819397348cd0 139
mitchpang 0:bb537a3c7f2a 140 //Create Regrind
mitchpang 0:bb537a3c7f2a 141 regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0);
mitchpang 2:819397348cd0 142 startColor = 1;
mitchpang 2:819397348cd0 143 //wait(1);
mitchpang 0:bb537a3c7f2a 144 } //if(og1...)
mitchpang 2:819397348cd0 145 else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 146 og1Oneshot = 0;
mitchpang 0:bb537a3c7f2a 147
mitchpang 0:bb537a3c7f2a 148 }//else if(og1 ...)
mitchpang 0:bb537a3c7f2a 149
mitchpang 2:819397348cd0 150 if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){
mitchpang 0:bb537a3c7f2a 151 og2Oneshot = 1;
mitchpang 3:597f8492fee2 152 divertParticle.rise(NULL);
mitchpang 0:bb537a3c7f2a 153 regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
mitchpang 0:bb537a3c7f2a 154 led3 = 1;
mitchpang 2:819397348cd0 155 if(regrindArray[og2Ndx].divert == 1){
mitchpang 2:819397348cd0 156 solenoid = 1; //actuate solenoid if red
mitchpang 2:819397348cd0 157 }
mitchpang 2:819397348cd0 158 else {
mitchpang 2:819397348cd0 159 solenoid = 0; //Dont actuate if not red.
mitchpang 2:819397348cd0 160 }
mitchpang 0:bb537a3c7f2a 161 }//if(og2..)
mitchpang 2:819397348cd0 162 else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 163 og2Oneshot = 0;
mitchpang 0:bb537a3c7f2a 164
mitchpang 0:bb537a3c7f2a 165 }//else if(og2 ...)
mitchpang 3:597f8492fee2 166 /*
mitchpang 2:819397348cd0 167 if((og3_adc == 0) && (og3Oneshot != 1)){
mitchpang 0:bb537a3c7f2a 168 og3Oneshot = 1;
mitchpang 0:bb537a3c7f2a 169 led4 = 1;
mitchpang 0:bb537a3c7f2a 170 regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
mitchpang 0:bb537a3c7f2a 171 if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid.
mitchpang 0:bb537a3c7f2a 172 solenoid = 1;
mitchpang 0:bb537a3c7f2a 173 }
mitchpang 0:bb537a3c7f2a 174 }//if(og3..)
mitchpang 2:819397348cd0 175 else if(og3_adc == 1){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 176 og3Oneshot = 0;
mitchpang 0:bb537a3c7f2a 177
mitchpang 0:bb537a3c7f2a 178 }//else if(og3 ...)
mitchpang 3:597f8492fee2 179 */
mitchpang 0:bb537a3c7f2a 180 //Check on 1pps clock
mitchpang 0:bb537a3c7f2a 181 if((totalT.read_us() % 2000000) < 1000000){
mitchpang 0:bb537a3c7f2a 182 led1 = 1;
mitchpang 0:bb537a3c7f2a 183 onePPS_out = 1;
mitchpang 0:bb537a3c7f2a 184 }
mitchpang 0:bb537a3c7f2a 185 else {//timer is in off cycle
mitchpang 0:bb537a3c7f2a 186 led1 = 0;
mitchpang 0:bb537a3c7f2a 187 onePPS_out = 0;
mitchpang 0:bb537a3c7f2a 188 }
mitchpang 0:bb537a3c7f2a 189
mitchpang 0:bb537a3c7f2a 190 //Check if data writing flag is set - if so, write to SD card
mitchpang 0:bb537a3c7f2a 191
mitchpang 0:bb537a3c7f2a 192 //Adjust PWM as necessary
mitchpang 3:597f8492fee2 193 augerMotors.Dir1 = reverseMotor1pb;
mitchpang 3:597f8492fee2 194 augerMotors.setpwm1pulsewidth(topMotorAdjuster.read());
mitchpang 3:597f8492fee2 195 augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read());
mitchpang 2:819397348cd0 196 //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
mitchpang 0:bb537a3c7f2a 197 } //while(1)
mitchpang 0:bb537a3c7f2a 198 totalT.stop();
mitchpang 0:bb537a3c7f2a 199 }// int main()