Diff: main.cpp

Revision:

6:ad8a8c14b1f0

Parent:

5:893aa74ff5aa

--- a/main.cpp	Sun Dec 06 18:53:38 2015 +0000
+++ b/main.cpp	Wed Dec 09 20:17:27 2015 +0000
@@ -2,8 +2,9 @@
 #include "Regrind.h"
 #include "RioRandHBridge.h"
 #include "Solenoid.h"
+#include "SDFileSystem.h"
 
-#define OG1_TO_OG2_DIST 1
+#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
@@ -15,7 +16,7 @@
 
 Solenoid led1(LED1, LED_ON_DELAY, LED_OFF_DELAY); //Used as 1pps out indicator
 Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
-Solenoid led3(LED3, 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)
@@ -23,18 +24,19 @@
 DigitalIn reverseMotor1pb(p26);
 DigitalIn reverseMotor2pb(p24);
 AnalogIn topMotorAdjuster(p19);
-DigitalOut bottomMotorAdjuster(p16,0);
+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);
-//DigitalIn redColor(p5);
-DigitalIn blueColor(p7);
-DigitalIn greenColor(p8);
-//Ticker 1pps;
+//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
 
@@ -50,81 +52,99 @@
 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;
     led3 = 1;
     }
-void reverseMotor1(){
-    augerMotors.Dir1 = !augerMotors.Dir1;
-    }
-void reverseMotor2(){
-    augerMotors.Dir2 = !augerMotors.Dir2;
-    }
 
 int main() {
+    
     //Start Clock
     totalT.start();
+    
     //Setup Information
-    /*
-    led1 = 0;
-    1pps_out = 0;
-    1pps.attach(&flip, 1.0);
-    */
+
+    //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 );
-    //reverseMotor1pb.attach_asserted( &reverseMotor1 );
-    //reverseMotor2pb.attach_asserted (&reverseMotor2);
     augerMotors.setpwm1pulsewidth(0.0);
     augerMotors.setpwm2pulsewidth(0.0);
     augerMotors.motor1_ccw();
     augerMotors.motor2_ccw();
+    fprintf(fp, "Motors Setup\r\n");
     
-    //Spin up Motors until fluctuation of 10% seen
-    
+    //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 spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min
+    //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\n\r",og1_calibration);
-    printf("og2_calibration value: %f\n\r",og2_calibration);
+    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);
+    //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;
@@ -139,8 +159,12 @@
             led1 = 1;
             
             //Create Regrind
-            regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0);
-            startColor = 1;
+            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
@@ -153,8 +177,27 @@
             divertParticle.rise(NULL);
             regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
             led2 = 1;
-            if(regrindArray[og2Ndx].divert == 1){
-            wait_ms(90);
+            
+            //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;
             }
@@ -163,7 +206,7 @@
             solenoid = 0; //Dont actuate if not red.
             led4 = 0;
             }
-            regrindArray[og2Ndx].divert = 0; //reset divert flag
+            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;
@@ -183,16 +226,16 @@
             
         }//else if(og3 ...)
         */
-        /*
+        
         //Check on 1pps clock
         if((totalT.read_us() % 2000000) < 1000000){
-            led1 = 1;
+            led3 = 1;
             onePPS_out = 1;
         }
         else {//timer is in off cycle
-            led1 = 0;
+            led3 = 0;
             onePPS_out = 0;
-        }*/
+        }
         
         //Check if data writing flag is set - if so, write to SD card
         
@@ -203,4 +246,5 @@
         //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
     } //while(1)
     totalT.stop();
+    //fclose(fp);
 }// int main()