main.cpp

00001 #include "mbed.h"
00002 #include "Regrind.h"
00003 #include "RioRandHBridge.h"
00004 #include "Solenoid.h"
00005 
00006 #define OG1_TO_OG2_DIST 1
00007 #define OG1_TO_OG3_DIST 2
00008 #define SOLENOID_ON_DELAY 0.25
00009 #define SOLENOID_OFF_DELAY 0.0
00010 #define LED_ON_DELAY 0.5
00011 #define LED_OFF_DELAY 0.0
00012 #define REGRIND_ARRAY_SIZE 1
00013 #define THRESHOLD 0.02
00014 #define RETURN_THRESHOLD 0.01
00015 
00016 Solenoid led1(LED1, LED_ON_DELAY, LED_OFF_DELAY); //Used as 1pps out indicator
00017 Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
00018 Solenoid led3(LED3, LED_ON_DELAY, LED_OFF_DELAY);
00019 Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY);
00020 DigitalOut onePPS_out(p29);
00021 Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay)
00022 RioRandHBridge augerMotors(p21, p25, p22, p23); //RioRandHBridge( PinName pinPwm1, PinName pinDir1, PinName pinPwm2, PinName pinDir2);
00023 DigitalIn reverseMotor1pb(p26);
00024 DigitalIn reverseMotor2pb(p24);
00025 AnalogIn topMotorAdjuster(p19);
00026 DigitalOut bottomMotorAdjuster(p16,0);
00027 DigitalOut unused1(p17,0);
00028 AnalogIn og1(p16);
00029 AnalogIn og2(p20);
00030 DigitalOut og3(p18,0);
00031 InterruptIn divertParticle(p5);
00032 Timer totalT;
00033 DigitalOut startColor(p6);
00034 //DigitalIn redColor(p5);
00035 DigitalIn blueColor(p7);
00036 DigitalIn greenColor(p8);
00037 //Ticker 1pps;
00038 
00039 Serial pc(USBTX,USBRX); //used for debugging
00040 
00041 float og1Threshold = 0.3;
00042 float og2Threshold = 0;
00043 float og3Threshold = 0;
00044 int   og1Oneshot = 0;
00045 int   og2Oneshot = 0;
00046 int   og3Oneshot = 0;
00047 float og1_adc = 0;
00048 float og2_adc = 0;
00049 float og3_adc = 0;
00050 int   og1Ndx = 0;
00051 int   og2Ndx = 0;
00052 int   og3Ndx = 0;
00053 float og1_min = 0;
00054 float og1_max = 2.7;
00055 float og2_min = 0;
00056 float og2_max = 0;
00057 float og3_min = 0;
00058 float og3_max = 0;
00059 float og1_calibration = 0;
00060 float og2_calibration = 0;
00061 float og3_calibration = 0;
00062 
00063 Regrind regrindArray[REGRIND_ARRAY_SIZE];
00064 /*
00065 void flip(){
00066     led1 = !led1;
00067     1pps_out = !1pps_out;
00068     }
00069 */
00070 void divert(){
00071     regrindArray[og2Ndx].divert = 1;
00072     led3 = 1;
00073     }
00074 void reverseMotor1(){
00075     augerMotors.Dir1 = !augerMotors.Dir1;
00076     }
00077 void reverseMotor2(){
00078     augerMotors.Dir2 = !augerMotors.Dir2;
00079     }
00080 
00081 int main() {
00082     //Start Clock
00083     totalT.start();
00084     //Setup Information
00085     /*
00086     led1 = 0;
00087     1pps_out = 0;
00088     1pps.attach(&flip, 1.0);
00089     */
00090     //Setup motors
00091     reverseMotor1pb.mode( PullUp );
00092     reverseMotor2pb.mode( PullUp );
00093     //reverseMotor1pb.attach_asserted( &reverseMotor1 );
00094     //reverseMotor2pb.attach_asserted (&reverseMotor2);
00095     augerMotors.setpwm1pulsewidth(0.0);
00096     augerMotors.setpwm2pulsewidth(0.0);
00097     augerMotors.motor1_ccw();
00098     augerMotors.motor2_ccw();
00099     
00100     //Spin up Motors until fluctuation of 10% seen
00101     
00102     
00103     //Calibrate the ADC
00104     //Done by spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min
00105     for(int i = 0; i<100;++i){
00106     og1_calibration += og1*3.3;
00107     og2_calibration += og2*3.3;
00108     }
00109     og1_calibration = og1_calibration/100;
00110     og2_calibration = og2_calibration/100;
00111     printf("og1_calibration value: %f\n\r",og1_calibration);
00112     printf("og2_calibration value: %f\n\r",og2_calibration);
00113     wait(3);
00114     
00115     divertParticle.rise(&divert);
00116     
00117     while(1) {
00118         /*
00119         if(redColor == 1){
00120             solenoid = 1; //actuate solenoid if red
00121             led1=1;
00122             }
00123             else {
00124             solenoid = 0; //actuate solenoid no matter what.
00125             led1=0;
00126             }
00127             */
00128         //Sample ADCs
00129         og1_adc = og1.read()*3.3;
00130         og2_adc = og2.read()*3.3;
00131         //og3_adc = og3.read()*3.3;
00132         //wait(0.01);
00133         //pc.printf("og1: %f og2: %f \n\r",og1_adc, og2_adc);
00134 
00135         if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
00136             og1Oneshot = 1;
00137             divertParticle.rise(&divert);
00138             //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
00139             led1 = 1;
00140             
00141             //Create Regrind
00142             regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0);
00143             startColor = 1;
00144             //wait(1);
00145         } //if(og1...)
00146         else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
00147             og1Oneshot = 0;
00148             
00149         }//else if(og1 ...)
00150         
00151         if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){
00152             og2Oneshot = 1;
00153             divertParticle.rise(NULL);
00154             regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
00155             led2 = 1;
00156             if(regrindArray[og2Ndx].divert == 1){
00157             wait_ms(90);
00158             solenoid = 1; //actuate solenoid if red
00159             led4 = 1;
00160             }
00161             else {
00162             //wait_ms(90);
00163             solenoid = 0; //Dont actuate if not red.
00164             led4 = 0;
00165             }
00166             regrindArray[og2Ndx].divert = 0; //reset divert flag
00167         }//if(og2..)
00168         else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
00169             og2Oneshot = 0;
00170             
00171         }//else if(og2 ...)
00172         /*
00173         if((og3_adc == 0) && (og3Oneshot != 1)){
00174             og3Oneshot = 1;
00175             led4 = 1;
00176             regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
00177             if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid.
00178                 solenoid = 1;
00179             }
00180         }//if(og3..)
00181         else if(og3_adc == 1){ //Regrind has passed ok to reset og
00182             og3Oneshot = 0;
00183             
00184         }//else if(og3 ...)
00185         */
00186         /*
00187         //Check on 1pps clock
00188         if((totalT.read_us() % 2000000) < 1000000){
00189             led1 = 1;
00190             onePPS_out = 1;
00191         }
00192         else {//timer is in off cycle
00193             led1 = 0;
00194             onePPS_out = 0;
00195         }*/
00196         
00197         //Check if data writing flag is set - if so, write to SD card
00198         
00199         //Adjust PWM as necessary
00200         augerMotors.Dir1 = reverseMotor1pb;
00201         augerMotors.setpwm1pulsewidth(topMotorAdjuster.read());
00202         augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read());
00203         //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
00204     } //while(1)
00205     totalT.stop();
00206 }// int main()