Diff: main.cpp

Revision:

2:819397348cd0

Parent:

0:bb537a3c7f2a

Child:

3:597f8492fee2

--- a/main.cpp	Tue Nov 24 09:49:58 2015 +0000
+++ b/main.cpp	Wed Dec 02 15:43:38 2015 +0000
@@ -5,11 +5,13 @@
 
 #define OG1_TO_OG2_DIST 1
 #define OG1_TO_OG3_DIST 2
-#define SOLENOID_ON_DELAY 0.001
+#define SOLENOID_ON_DELAY 0.5
 #define SOLENOID_OFF_DELAY 0.0
 #define LED_ON_DELAY 0.5
 #define LED_OFF_DELAY 0.0
 #define REGRIND_ARRAY_SIZE 1
+#define THRESHOLD 0.015
+#define RETURN_THRESHOLD 0.01
 
 DigitalOut led1(LED1,0); //Used as 1pps out indicator
 Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
@@ -18,18 +20,23 @@
 DigitalOut onePPS_out(p29);
 Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay)
 TB6612FNG augerMotors(p21, p26, p25, p22, p24, p23, p27); //TB6612FNG( PinName pinPwmA, PinName pinAin1, PinName pinAin2, PinName pinPwmB, PinName pinBin1, PinName pinBin2, PinName pinNStby );
-AnalogIn topMotorAdjuster(p15);
+AnalogIn topMotorAdjuster(p19);
 AnalogIn bottomMotorAdjuster(p16);
-DigitalOut unused1(p17);
-AnalogIn og1(p20);
-AnalogIn og2(p19);
+DigitalOut unused1(p17,0);
+AnalogIn og1(p15);
+AnalogIn og2(p20);
 AnalogIn og3(p18);
+InterruptIn divertParticle(p5);
 Timer totalT;
+DigitalOut startColor(p6);
+//DigitalIn redColor(p5);
+DigitalIn blueColor(p7);
+DigitalIn greenColor(p8);
 //Ticker 1pps;
 
 Serial pc(USBTX,USBRX); //used for debugging
 
-float og1Threshold = 0;
+float og1Threshold = 0.3;
 float og2Threshold = 0;
 float og3Threshold = 0;
 int   og1Oneshot = 0;
@@ -42,11 +49,14 @@
 int   og2Ndx = 0;
 int   og3Ndx = 0;
 float og1_min = 0;
-float og1_max = 0;
+float og1_max = 2.7;
 float og2_min = 0;
 float og2_max = 0;
 float og3_min = 0;
 float og3_max = 0;
+float og1_calibration = 0;
+float og2_calibration = 0;
+float og3_calibration = 0;
 
 Regrind regrindArray[REGRIND_ARRAY_SIZE];
 /*
@@ -55,6 +65,9 @@
     1pps_out = !1pps_out;
     }
 */
+void divert(){
+    regrindArray[og2Ndx].divert = 1;
+    }
 
 int main() {
     //Start Clock
@@ -76,37 +89,68 @@
     
     //Calibrate the ADC
     //Done by spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min
+    for(int i = 0; i<100;++i){
+    og1_calibration += og1*3.3;
+    og2_calibration += og2*3.3;
+    }
+    og1_calibration = og1_calibration/100;
+    og2_calibration = og2_calibration/100;
+    printf("og1_calibration value: %f\n\r",og1_calibration);
+    printf("og2_calibration value: %f\n\r",og2_calibration);
+    wait(3);
+    
+    divertParticle.rise(&divert);
     
     while(1) {
+        /*
+        if(redColor == 1){
+            solenoid = 1; //actuate solenoid if red
+            led1=1;
+            }
+            else {
+            solenoid = 0; //actuate solenoid no matter what.
+            led1=0;
+            }
+            */
         //Sample ADCs
-        og1_adc = og1;
-        og2_adc = og2;
-        og3_adc = og3;
+        og1_adc = og1.read()*3.3;
+        og2_adc = og2.read()*3.3;
+        og3_adc = og3.read()*3.3;
+        //wait(0.01);
+        //pc.printf("og1: %f og2: %f og3: %f\n\r",og1_adc, og2_adc, og3_adc);
         
-        if((og1_adc < og1Threshold) && (og1Oneshot != 1)){ //Something passed through og1
+        if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
             og1Oneshot = 1;
-            pc.printf("Regrind seen at OG 1 : %dV\n\r", 3.3*og1_adc);
+            //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
             led2 = 1;
+            
             //Create Regrind
             regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0);
-            
+            startColor = 1;
+            //wait(1);
         } //if(og1...)
-        else if(og1_adc >  0.95*og1_max){ //Regrind has passed ok to reset og
+        else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
             og1Oneshot = 0;
             
         }//else if(og1 ...)
         
-        if((og2_adc < og2Threshold) && (og2Oneshot != 1)){
+        if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){
             og2Oneshot = 1;
             regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
             led3 = 1;
+            if(regrindArray[og2Ndx].divert == 1){
+            solenoid = 1; //actuate solenoid if red
+            }
+            else {
+            solenoid = 0; //Dont actuate if not red.
+            }
         }//if(og2..)
-        else if(og2_adc >  0.95*og2_max){ //Regrind has passed ok to reset og
+        else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
             og2Oneshot = 0;
             
         }//else if(og2 ...)
         
-        if((og3_adc < og3Threshold) && (og3Oneshot != 1)){
+        if((og3_adc == 0) && (og3Oneshot != 1)){
             og3Oneshot = 1;
             led4 = 1;
             regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
@@ -114,7 +158,7 @@
                 solenoid = 1;
             }
         }//if(og3..)
-        else if(og3_adc >  0.95*og3_max){ //Regrind has passed ok to reset og
+        else if(og3_adc == 1){ //Regrind has passed ok to reset og
             og3Oneshot = 0;
             
         }//else if(og3 ...)
@@ -134,6 +178,7 @@
         //Adjust PWM as necessary
         augerMotors.setPwmApulsewidth(topMotorAdjuster.read());
         augerMotors.setPwmBpulsewidth(bottomMotorAdjuster.read());
+        //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
     } //while(1)
     totalT.stop();
 }// int main()