File content as of revision 6:ad8a8c14b1f0:

#include "mbed.h"
#include "Regrind.h"
#include "RioRandHBridge.h"
#include "Solenoid.h"
#include "SDFileSystem.h"

#define OG1_TO_OG2_DIST 0.127
#define OG1_TO_OG3_DIST 2
#define SOLENOID_ON_DELAY 0.25
#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.02
#define RETURN_THRESHOLD 0.01

Solenoid led1(LED1, LED_ON_DELAY, LED_OFF_DELAY); //Used as 1pps out indicator
Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
DigitalOut led3(LED3);
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)
RioRandHBridge augerMotors(p21, p25, p22, p23); //RioRandHBridge( PinName pinPwm1, PinName pinDir1, PinName pinPwm2, PinName pinDir2);
DigitalIn reverseMotor1pb(p26);
DigitalIn reverseMotor2pb(p24);
AnalogIn topMotorAdjuster(p19);
DigitalOut bottomMotorAdjuster(p15,0);
DigitalOut unused1(p17,0);
AnalogIn og1(p16);
AnalogIn og2(p20);
DigitalOut og3(p18,0);
InterruptIn divertParticle(p5);
Timer totalT;
//DigitalOut startColor(p6);

SPI sensorModule(p11, p12, p13); //mosi, miso, sclk
DigitalOut sensorModule_cs(p14, 1); //Sensor Module Chip Select Active low when requesting chip

SDFileSystem sd(p11, p12, p13, p8, "sd"); // the pinout on the mbed Cool Components workshop board

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_calibration = 0;
float og2_calibration = 0;
float og3_calibration = 0;

Regrind regrindArray[REGRIND_ARRAY_SIZE];


void divert(){
    regrindArray[og2Ndx].divert = 1;
    led3 = 1;
    }

int main() {
    
    //Start Clock
    totalT.start();
    
    //Setup Information

    //Setup sd Card Data Logger
    mkdir("/sd/PHD", 0777);
    FILE *fp = fopen("/sd/PHD/log.txt", "w");
    //FileHandle *fp = sd.open("PHD/log.txt", O_WRONLY|O_CREAT|O_TRUNC);
    if(fp == NULL) {
        printf("Could not open file for write\r\n");
        reverseMotor1pb.mode( PullUp );
        augerMotors.setpwm1pulsewidth(0.0);
        augerMotors.motor1_ccw();
        while(1){
            led1 = 1;
            wait(1);
            led1 = 0;
            }
    }
    fprintf(fp, "Initalizing PHD...\r\n");
    //char buffer[1] = {'a'};
    //fp->write(buffer,sizeof(buffer));
    //fp->close();
    //Setup motors
    reverseMotor1pb.mode( PullUp );
    reverseMotor2pb.mode( PullUp );
    augerMotors.setpwm1pulsewidth(0.0);
    augerMotors.setpwm2pulsewidth(0.0);
    augerMotors.motor1_ccw();
    augerMotors.motor2_ccw();
    fprintf(fp, "Motors Setup\r\n");
    
    //Setup SPI communication to modular sensor module
    // Setup the spi for 8 bit data, high steady state clock,
    // second edge capture, with a 1MHz clock rate
    /*
    sensorModule.format(8,3);
    sensorModule.frequency(1000000);
    sensorModule_cs = 0; //Select sensor module
    wait(1);
    char r = sensorModule.write(0x8F);
    r = sensorModule.write(0x00);
    r = sensorModule.write(0x01);
    pc.printf("%x\n",r);
    if(r != 0x89){//SPI Sensor Module Not Connected
        pc.printf("Could not find SPI sensor module\n\r");
        fprintf(fp, "Could not find SPI sensor module ... Setting endless distinctive LED Pattern.\n");
        
        while(1) {
            led1 = led2 = led4 = 1;
            wait(0.5);
            led1 = led2 = led4 = 0;
        }
    }
    sensorModule_cs = 1; //deselect sensor module
    fprintf(fp, "SPI communication to sensor module achieved\n");
    */
    
    //Calibrate the ADC
    //Done by averaging 100 samples of adc
    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\r\n",og1_calibration);
    printf("og2_calibration value: %f\r\n",og2_calibration);
    fprintf(fp,"og1_calibration value: %f\r\n",og1_calibration);
    fprintf(fp,"og2_calibration value: %f\r\n",og2_calibration);
    fprintf(fp,"Initialization Complete\r\n");
    fprintf(fp,"Regrind#\tTimeSeen(us)\tVelocity(m/s)\tAcceleration(m/s2)\tdivert?\r\n");
    fclose(fp);
    wait(3);
    
    //divertParticle.rise(&divert);
    
    while(1) {
        //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 \n\r",og1_adc, og2_adc);

        if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
            og1Oneshot = 1;
            divertParticle.rise(&divert);
            //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
            led1 = 1;
            
            //Create Regrind
            regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0); //Regrind(double timeSeen, int location, double velocity, double a, int d, int pD);
            //Send Regrind Position & Time Data
            //sensorModule_cs = 0; //Select sensor module
            //sensorModule.write((og1Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x0);//Tell Sensor PHD is about to send time data.
            //sensorModule.write(regrindArray[og1Ndx%REGRIND_ARRAY_SIZE].velocity); //Write velocity data to sensor.
            //sensorModule_cs = 1; //Deselect sensor module
            //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;
            divertParticle.rise(NULL);
            regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
            led2 = 1;
            
            //Send Regrind Velocity Data
            //sensorModule_cs = 0; //Select sensor module
            //sensorModule.write((og2Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x1);//Tell Sensor PHD is about to send velocity data.
            //sensorModule.write(regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].velocity); //Write velocity data to sensor.
            //sensorModule_cs = 1; //Deselect sensor module
            
            wait_ms(45);//Give time for sensor to collect data
            
            //Get regrind divert data
            //sensorModule_cs = 0; //Select sensor module
            //sensorModule.write((og2Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x3);//Tell Sensor we need divert data.
            //regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert = sensorModule.write(0x00000000);
            //sensorModule_cs = 1; //Deselect sensor module
            
            //Record data to SD card
            FILE *fp = fopen("/sd/PHD/log.txt", "a");
            fprintf(fp,"%d\t%d\t%f\t%f\t%d\r\n",og2Ndx%REGRIND_ARRAY_SIZE,regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].timeSeen,regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].velocity,regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].acceleration,regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert);
            fclose(fp);
            
            if(regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert == 1){
            solenoid = 1; //actuate solenoid if red
            led4 = 1;
            }
            else {
            //wait_ms(90);
            solenoid = 0; //Dont actuate if not red.
            led4 = 0;
            }
            regrindArray[og2Ndx].clearRegrind(); //reset divert flag
        }//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){
            led3 = 1;
            onePPS_out = 1;
        }
        else {//timer is in off cycle
            led3 = 0;
            onePPS_out = 0;
        }
        
        //Check if data writing flag is set - if so, write to SD card
        
        //Adjust PWM as necessary
        augerMotors.Dir1 = reverseMotor1pb;
        augerMotors.setpwm1pulsewidth(topMotorAdjuster.read());
        augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read());
        //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
    } //while(1)
    totalT.stop();
    //fclose(fp);
}// int main()