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
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()
Generated on Thu Aug 4 2022 13:53:39 by 1.7.2