main.cpp

00001 #include "mbed.h"
00002 #include "Regrind.h"
00003 #include "RioRandHBridge.h"
00004 #include "Solenoid.h"
00005 #include "SDFileSystem.h"
00006 
00007 #define OG1_TO_OG2_DIST 0.127
00008 #define OG1_TO_OG3_DIST 2
00009 #define SOLENOID_ON_DELAY 0.25
00010 #define SOLENOID_OFF_DELAY 0.0
00011 #define LED_ON_DELAY 0.5
00012 #define LED_OFF_DELAY 0.0
00013 #define REGRIND_ARRAY_SIZE 1
00014 #define THRESHOLD 0.02
00015 #define RETURN_THRESHOLD 0.01
00016 
00017 Solenoid led1(LED1, LED_ON_DELAY, LED_OFF_DELAY); //Used as 1pps out indicator
00018 Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
00019 DigitalOut led3(LED3);
00020 Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY);
00021 DigitalOut onePPS_out(p29);
00022 Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay)
00023 RioRandHBridge augerMotors(p21, p25, p22, p23); //RioRandHBridge( PinName pinPwm1, PinName pinDir1, PinName pinPwm2, PinName pinDir2);
00024 DigitalIn reverseMotor1pb(p26);
00025 DigitalIn reverseMotor2pb(p24);
00026 AnalogIn topMotorAdjuster(p19);
00027 DigitalOut bottomMotorAdjuster(p15,0);
00028 DigitalOut unused1(p17,0);
00029 AnalogIn og1(p16);
00030 AnalogIn og2(p20);
00031 DigitalOut og3(p18,0);
00032 InterruptIn divertParticle(p5);
00033 Timer totalT;
00034 //DigitalOut startColor(p6);
00035 
00036 SPI sensorModule(p11, p12, p13); //mosi, miso, sclk
00037 DigitalOut sensorModule_cs(p14, 1); //Sensor Module Chip Select Active low when requesting chip
00038 
00039 SDFileSystem sd(p11, p12, p13, p8, "sd"); // the pinout on the mbed Cool Components workshop board
00040 
00041 Serial pc(USBTX,USBRX); //used for debugging
00042 
00043 float og1Threshold = 0.3;
00044 float og2Threshold = 0;
00045 float og3Threshold = 0;
00046 int   og1Oneshot = 0;
00047 int   og2Oneshot = 0;
00048 int   og3Oneshot = 0;
00049 float og1_adc = 0;
00050 float og2_adc = 0;
00051 float og3_adc = 0;
00052 int   og1Ndx = 0;
00053 int   og2Ndx = 0;
00054 int   og3Ndx = 0;
00055 float og1_calibration = 0;
00056 float og2_calibration = 0;
00057 float og3_calibration = 0;
00058 
00059 Regrind regrindArray[REGRIND_ARRAY_SIZE];
00060 
00061 
00062 void divert(){
00063     regrindArray[og2Ndx].divert = 1;
00064     led3 = 1;
00065     }
00066 
00067 int main() {
00068     
00069     //Start Clock
00070     totalT.start();
00071     
00072     //Setup Information
00073 
00074     //Setup sd Card Data Logger
00075     mkdir("/sd/PHD", 0777);
00076     FILE *fp = fopen("/sd/PHD/log.txt", "w");
00077     //FileHandle *fp = sd.open("PHD/log.txt", O_WRONLY|O_CREAT|O_TRUNC);
00078     if(fp == NULL) {
00079         printf("Could not open file for write\r\n");
00080         reverseMotor1pb.mode( PullUp );
00081         augerMotors.setpwm1pulsewidth(0.0);
00082         augerMotors.motor1_ccw();
00083         while(1){
00084             led1 = 1;
00085             wait(1);
00086             led1 = 0;
00087             }
00088     }
00089     fprintf(fp, "Initalizing PHD...\r\n");
00090     //char buffer[1] = {'a'};
00091     //fp->write(buffer,sizeof(buffer));
00092     //fp->close();
00093     //Setup motors
00094     reverseMotor1pb.mode( PullUp );
00095     reverseMotor2pb.mode( PullUp );
00096     augerMotors.setpwm1pulsewidth(0.0);
00097     augerMotors.setpwm2pulsewidth(0.0);
00098     augerMotors.motor1_ccw();
00099     augerMotors.motor2_ccw();
00100     fprintf(fp, "Motors Setup\r\n");
00101     
00102     //Setup SPI communication to modular sensor module
00103     // Setup the spi for 8 bit data, high steady state clock,
00104     // second edge capture, with a 1MHz clock rate
00105     /*
00106     sensorModule.format(8,3);
00107     sensorModule.frequency(1000000);
00108     sensorModule_cs = 0; //Select sensor module
00109     wait(1);
00110     char r = sensorModule.write(0x8F);
00111     r = sensorModule.write(0x00);
00112     r = sensorModule.write(0x01);
00113     pc.printf("%x\n",r);
00114     if(r != 0x89){//SPI Sensor Module Not Connected
00115         pc.printf("Could not find SPI sensor module\n\r");
00116         fprintf(fp, "Could not find SPI sensor module ... Setting endless distinctive LED Pattern.\n");
00117         
00118         while(1) {
00119             led1 = led2 = led4 = 1;
00120             wait(0.5);
00121             led1 = led2 = led4 = 0;
00122         }
00123     }
00124     sensorModule_cs = 1; //deselect sensor module
00125     fprintf(fp, "SPI communication to sensor module achieved\n");
00126     */
00127     
00128     //Calibrate the ADC
00129     //Done by averaging 100 samples of adc
00130     for(int i = 0; i<100;++i){
00131     og1_calibration += og1*3.3;
00132     og2_calibration += og2*3.3;
00133     }
00134     og1_calibration = og1_calibration/100;
00135     og2_calibration = og2_calibration/100;
00136     printf("og1_calibration value: %f\r\n",og1_calibration);
00137     printf("og2_calibration value: %f\r\n",og2_calibration);
00138     fprintf(fp,"og1_calibration value: %f\r\n",og1_calibration);
00139     fprintf(fp,"og2_calibration value: %f\r\n",og2_calibration);
00140     fprintf(fp,"Initialization Complete\r\n");
00141     fprintf(fp,"Regrind#\tTimeSeen(us)\tVelocity(m/s)\tAcceleration(m/s2)\tdivert?\r\n");
00142     fclose(fp);
00143     wait(3);
00144     
00145     //divertParticle.rise(&divert);
00146     
00147     while(1) {
00148         //Sample ADCs
00149         og1_adc = og1.read()*3.3;
00150         og2_adc = og2.read()*3.3;
00151         //og3_adc = og3.read()*3.3;
00152         //wait(0.01);
00153         //pc.printf("og1: %f og2: %f \n\r",og1_adc, og2_adc);
00154 
00155         if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
00156             og1Oneshot = 1;
00157             divertParticle.rise(&divert);
00158             //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
00159             led1 = 1;
00160             
00161             //Create Regrind
00162             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);
00163             //Send Regrind Position & Time Data
00164             //sensorModule_cs = 0; //Select sensor module
00165             //sensorModule.write((og1Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x0);//Tell Sensor PHD is about to send time data.
00166             //sensorModule.write(regrindArray[og1Ndx%REGRIND_ARRAY_SIZE].velocity); //Write velocity data to sensor.
00167             //sensorModule_cs = 1; //Deselect sensor module
00168             //wait(1);
00169         } //if(og1...)
00170         else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
00171             og1Oneshot = 0;
00172             
00173         }//else if(og1 ...)
00174         
00175         if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){
00176             og2Oneshot = 1;
00177             divertParticle.rise(NULL);
00178             regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
00179             led2 = 1;
00180             
00181             //Send Regrind Velocity Data
00182             //sensorModule_cs = 0; //Select sensor module
00183             //sensorModule.write((og2Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x1);//Tell Sensor PHD is about to send velocity data.
00184             //sensorModule.write(regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].velocity); //Write velocity data to sensor.
00185             //sensorModule_cs = 1; //Deselect sensor module
00186             
00187             wait_ms(45);//Give time for sensor to collect data
00188             
00189             //Get regrind divert data
00190             //sensorModule_cs = 0; //Select sensor module
00191             //sensorModule.write((og2Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x3);//Tell Sensor we need divert data.
00192             //regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert = sensorModule.write(0x00000000);
00193             //sensorModule_cs = 1; //Deselect sensor module
00194             
00195             //Record data to SD card
00196             FILE *fp = fopen("/sd/PHD/log.txt", "a");
00197             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);
00198             fclose(fp);
00199             
00200             if(regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert == 1){
00201             solenoid = 1; //actuate solenoid if red
00202             led4 = 1;
00203             }
00204             else {
00205             //wait_ms(90);
00206             solenoid = 0; //Dont actuate if not red.
00207             led4 = 0;
00208             }
00209             regrindArray[og2Ndx].clearRegrind(); //reset divert flag
00210         }//if(og2..)
00211         else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
00212             og2Oneshot = 0;
00213             
00214         }//else if(og2 ...)
00215         /*
00216         if((og3_adc == 0) && (og3Oneshot != 1)){
00217             og3Oneshot = 1;
00218             led4 = 1;
00219             regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
00220             if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid.
00221                 solenoid = 1;
00222             }
00223         }//if(og3..)
00224         else if(og3_adc == 1){ //Regrind has passed ok to reset og
00225             og3Oneshot = 0;
00226             
00227         }//else if(og3 ...)
00228         */
00229         
00230         //Check on 1pps clock
00231         if((totalT.read_us() % 2000000) < 1000000){
00232             led3 = 1;
00233             onePPS_out = 1;
00234         }
00235         else {//timer is in off cycle
00236             led3 = 0;
00237             onePPS_out = 0;
00238         }
00239         
00240         //Check if data writing flag is set - if so, write to SD card
00241         
00242         //Adjust PWM as necessary
00243         augerMotors.Dir1 = reverseMotor1pb;
00244         augerMotors.setpwm1pulsewidth(topMotorAdjuster.read());
00245         augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read());
00246         //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
00247     } //while(1)
00248     totalT.stop();
00249     //fclose(fp);
00250 }// int main()