Added functionality to output stored data to sd card. Code for SPI communication to modular sensor array is commented out due to bugs in reading in SPI data from arduino. (Sending SPI data to arduino is working)
Dependencies: Regrind RioRandHBridge SDFileSystem Solenoid mbed
Fork of ECE4012-PHD by
main.cpp
- Committer:
- mitchpang
- Date:
- 2015-12-09
- Revision:
- 6:ad8a8c14b1f0
- Parent:
- 5:893aa74ff5aa
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()