File content as of revision 2:819397348cd0:

#include "mbed.h"
#include "Regrind.h"
#include "TB6612FNG.h"
#include "Solenoid.h"

#define OG1_TO_OG2_DIST 1
#define OG1_TO_OG3_DIST 2
#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);
Solenoid led3(LED3, LED_ON_DELAY, LED_OFF_DELAY);
Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY);
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(p19);
AnalogIn bottomMotorAdjuster(p16);
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.3;
float og2Threshold = 0;
float og3Threshold = 0;
int   og1Oneshot = 0;
int   og2Oneshot = 0;
int   og3Oneshot = 0;
float og1_adc = 0;
float og2_adc = 0;
float og3_adc = 0;
int   og1Ndx = 0;
int   og2Ndx = 0;
int   og3Ndx = 0;
float og1_min = 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];
/*
void flip(){
    led1 = !led1;
    1pps_out = !1pps_out;
    }
*/
void divert(){
    regrindArray[og2Ndx].divert = 1;
    }

int main() {
    //Start Clock
    totalT.start();
    //Setup Information
    /*
    led1 = 0;
    1pps_out = 0;
    1pps.attach(&flip, 1.0);
    */
    //Setup motors
    augerMotors.setPwmApulsewidth(0.0);
    augerMotors.setPwmBpulsewidth(0.0);
    augerMotors.motorA_ccw();
    augerMotors.motorB_ccw();
    
    //Spin up Motors until fluctuation of 10% seen
    
    
    //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.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_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
            og1Oneshot = 1;
            //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_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
            og1Oneshot = 0;
            
        }//else if(og1 ...)
        
        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_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
            og2Oneshot = 0;
            
        }//else if(og2 ...)
        
        if((og3_adc == 0) && (og3Oneshot != 1)){
            og3Oneshot = 1;
            led4 = 1;
            regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
            if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid.
                solenoid = 1;
            }
        }//if(og3..)
        else if(og3_adc == 1){ //Regrind has passed ok to reset og
            og3Oneshot = 0;
            
        }//else if(og3 ...)
        
        //Check on 1pps clock
        if((totalT.read_us() % 2000000) < 1000000){
            led1 = 1;
            onePPS_out = 1;
        }
        else {//timer is in off cycle
            led1 = 0;
            onePPS_out = 0;
        }
        
        //Check if data writing flag is set - if so, write to SD card
        
        //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()