Working copy of PHD program. Make any modifications to this file and archive in separate programs as necessary.
Dependencies: Regrind RioRandHBridge SDFileSystem Solenoid mbed
Fork of ECE4012-PHD_data_out_and_1pps 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()