Team Plastics Separation / Mbed 2 deprecated ECE4012-PHD_working_copy

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 Team Plastics Separation

main.cpp@6:ad8a8c14b1f0, 2015-12-09 (annotated)

Committer:
mitchpang
Date:
Wed Dec 09 20:17:27 2015 +0000
Revision:
6:ad8a8c14b1f0
Parent:
5:893aa74ff5aa 
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)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mitchpang 0:bb537a3c7f2a 1 #include "mbed.h"
mitchpang 0:bb537a3c7f2a 2 #include "Regrind.h"
mitchpang 3:597f8492fee2 3 #include "RioRandHBridge.h"
mitchpang 0:bb537a3c7f2a 4 #include "Solenoid.h"
mitchpang 6:ad8a8c14b1f0 5 #include "SDFileSystem.h"
mitchpang 0:bb537a3c7f2a 6
mitchpang 6:ad8a8c14b1f0 7 #define OG1_TO_OG2_DIST 0.127
mitchpang 0:bb537a3c7f2a 8 #define OG1_TO_OG3_DIST 2
mitchpang 5:893aa74ff5aa 9 #define SOLENOID_ON_DELAY 0.25
mitchpang 0:bb537a3c7f2a 10 #define SOLENOID_OFF_DELAY 0.0
mitchpang 0:bb537a3c7f2a 11 #define LED_ON_DELAY 0.5
mitchpang 0:bb537a3c7f2a 12 #define LED_OFF_DELAY 0.0
mitchpang 0:bb537a3c7f2a 13 #define REGRIND_ARRAY_SIZE 1
mitchpang 5:893aa74ff5aa 14 #define THRESHOLD 0.02
mitchpang 2:819397348cd0 15 #define RETURN_THRESHOLD 0.01
mitchpang 0:bb537a3c7f2a 16
mitchpang 5:893aa74ff5aa 17 Solenoid led1(LED1, LED_ON_DELAY, LED_OFF_DELAY); //Used as 1pps out indicator
mitchpang 0:bb537a3c7f2a 18 Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
mitchpang 6:ad8a8c14b1f0 19 DigitalOut led3(LED3);
mitchpang 0:bb537a3c7f2a 20 Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY);
mitchpang 0:bb537a3c7f2a 21 DigitalOut onePPS_out(p29);
mitchpang 0:bb537a3c7f2a 22 Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay)
mitchpang 3:597f8492fee2 23 RioRandHBridge augerMotors(p21, p25, p22, p23); //RioRandHBridge( PinName pinPwm1, PinName pinDir1, PinName pinPwm2, PinName pinDir2);
mitchpang 3:597f8492fee2 24 DigitalIn reverseMotor1pb(p26);
mitchpang 3:597f8492fee2 25 DigitalIn reverseMotor2pb(p24);
mitchpang 2:819397348cd0 26 AnalogIn topMotorAdjuster(p19);
mitchpang 6:ad8a8c14b1f0 27 DigitalOut bottomMotorAdjuster(p15,0);
mitchpang 2:819397348cd0 28 DigitalOut unused1(p17,0);
mitchpang 5:893aa74ff5aa 29 AnalogIn og1(p16);
mitchpang 2:819397348cd0 30 AnalogIn og2(p20);
mitchpang 3:597f8492fee2 31 DigitalOut og3(p18,0);
mitchpang 2:819397348cd0 32 InterruptIn divertParticle(p5);
mitchpang 0:bb537a3c7f2a 33 Timer totalT;
mitchpang 6:ad8a8c14b1f0 34 //DigitalOut startColor(p6);
mitchpang 6:ad8a8c14b1f0 35
mitchpang 6:ad8a8c14b1f0 36 SPI sensorModule(p11, p12, p13); //mosi, miso, sclk
mitchpang 6:ad8a8c14b1f0 37 DigitalOut sensorModule_cs(p14, 1); //Sensor Module Chip Select Active low when requesting chip
mitchpang 6:ad8a8c14b1f0 38
mitchpang 6:ad8a8c14b1f0 39 SDFileSystem sd(p11, p12, p13, p8, "sd"); // the pinout on the mbed Cool Components workshop board
mitchpang 0:bb537a3c7f2a 40
mitchpang 0:bb537a3c7f2a 41 Serial pc(USBTX,USBRX); //used for debugging
mitchpang 0:bb537a3c7f2a 42
mitchpang 2:819397348cd0 43 float og1Threshold = 0.3;
mitchpang 0:bb537a3c7f2a 44 float og2Threshold = 0;
mitchpang 0:bb537a3c7f2a 45 float og3Threshold = 0;
mitchpang 0:bb537a3c7f2a 46 int og1Oneshot = 0;
mitchpang 0:bb537a3c7f2a 47 int og2Oneshot = 0;
mitchpang 0:bb537a3c7f2a 48 int og3Oneshot = 0;
mitchpang 0:bb537a3c7f2a 49 float og1_adc = 0;
mitchpang 0:bb537a3c7f2a 50 float og2_adc = 0;
mitchpang 0:bb537a3c7f2a 51 float og3_adc = 0;
mitchpang 0:bb537a3c7f2a 52 int og1Ndx = 0;
mitchpang 0:bb537a3c7f2a 53 int og2Ndx = 0;
mitchpang 0:bb537a3c7f2a 54 int og3Ndx = 0;
mitchpang 2:819397348cd0 55 float og1_calibration = 0;
mitchpang 2:819397348cd0 56 float og2_calibration = 0;
mitchpang 2:819397348cd0 57 float og3_calibration = 0;
mitchpang 0:bb537a3c7f2a 58
mitchpang 0:bb537a3c7f2a 59 Regrind regrindArray[REGRIND_ARRAY_SIZE];
mitchpang 6:ad8a8c14b1f0 60
mitchpang 6:ad8a8c14b1f0 61
mitchpang 2:819397348cd0 62 void divert(){
mitchpang 2:819397348cd0 63 regrindArray[og2Ndx].divert = 1;
mitchpang 5:893aa74ff5aa 64 led3 = 1;
mitchpang 2:819397348cd0 65 }
mitchpang 0:bb537a3c7f2a 66
mitchpang 0:bb537a3c7f2a 67 int main() {
mitchpang 6:ad8a8c14b1f0 68
mitchpang 0:bb537a3c7f2a 69 //Start Clock
mitchpang 0:bb537a3c7f2a 70 totalT.start();
mitchpang 6:ad8a8c14b1f0 71
mitchpang 0:bb537a3c7f2a 72 //Setup Information
mitchpang 6:ad8a8c14b1f0 73
mitchpang 6:ad8a8c14b1f0 74 //Setup sd Card Data Logger
mitchpang 6:ad8a8c14b1f0 75 mkdir("/sd/PHD", 0777);
mitchpang 6:ad8a8c14b1f0 76 FILE *fp = fopen("/sd/PHD/log.txt", "w");
mitchpang 6:ad8a8c14b1f0 77 //FileHandle *fp = sd.open("PHD/log.txt", O_WRONLY|O_CREAT|O_TRUNC);
mitchpang 6:ad8a8c14b1f0 78 if(fp == NULL) {
mitchpang 6:ad8a8c14b1f0 79 printf("Could not open file for write\r\n");
mitchpang 6:ad8a8c14b1f0 80 reverseMotor1pb.mode( PullUp );
mitchpang 6:ad8a8c14b1f0 81 augerMotors.setpwm1pulsewidth(0.0);
mitchpang 6:ad8a8c14b1f0 82 augerMotors.motor1_ccw();
mitchpang 6:ad8a8c14b1f0 83 while(1){
mitchpang 6:ad8a8c14b1f0 84 led1 = 1;
mitchpang 6:ad8a8c14b1f0 85 wait(1);
mitchpang 6:ad8a8c14b1f0 86 led1 = 0;
mitchpang 6:ad8a8c14b1f0 87 }
mitchpang 6:ad8a8c14b1f0 88 }
mitchpang 6:ad8a8c14b1f0 89 fprintf(fp, "Initalizing PHD...\r\n");
mitchpang 6:ad8a8c14b1f0 90 //char buffer[1] = {'a'};
mitchpang 6:ad8a8c14b1f0 91 //fp->write(buffer,sizeof(buffer));
mitchpang 6:ad8a8c14b1f0 92 //fp->close();
mitchpang 0:bb537a3c7f2a 93 //Setup motors
mitchpang 3:597f8492fee2 94 reverseMotor1pb.mode( PullUp );
mitchpang 3:597f8492fee2 95 reverseMotor2pb.mode( PullUp );
mitchpang 3:597f8492fee2 96 augerMotors.setpwm1pulsewidth(0.0);
mitchpang 3:597f8492fee2 97 augerMotors.setpwm2pulsewidth(0.0);
mitchpang 3:597f8492fee2 98 augerMotors.motor1_ccw();
mitchpang 3:597f8492fee2 99 augerMotors.motor2_ccw();
mitchpang 6:ad8a8c14b1f0 100 fprintf(fp, "Motors Setup\r\n");
mitchpang 0:bb537a3c7f2a 101
mitchpang 6:ad8a8c14b1f0 102 //Setup SPI communication to modular sensor module
mitchpang 6:ad8a8c14b1f0 103 // Setup the spi for 8 bit data, high steady state clock,
mitchpang 6:ad8a8c14b1f0 104 // second edge capture, with a 1MHz clock rate
mitchpang 6:ad8a8c14b1f0 105 /*
mitchpang 6:ad8a8c14b1f0 106 sensorModule.format(8,3);
mitchpang 6:ad8a8c14b1f0 107 sensorModule.frequency(1000000);
mitchpang 6:ad8a8c14b1f0 108 sensorModule_cs = 0; //Select sensor module
mitchpang 6:ad8a8c14b1f0 109 wait(1);
mitchpang 6:ad8a8c14b1f0 110 char r = sensorModule.write(0x8F);
mitchpang 6:ad8a8c14b1f0 111 r = sensorModule.write(0x00);
mitchpang 6:ad8a8c14b1f0 112 r = sensorModule.write(0x01);
mitchpang 6:ad8a8c14b1f0 113 pc.printf("%x\n",r);
mitchpang 6:ad8a8c14b1f0 114 if(r != 0x89){//SPI Sensor Module Not Connected
mitchpang 6:ad8a8c14b1f0 115 pc.printf("Could not find SPI sensor module\n\r");
mitchpang 6:ad8a8c14b1f0 116 fprintf(fp, "Could not find SPI sensor module ... Setting endless distinctive LED Pattern.\n");
mitchpang 6:ad8a8c14b1f0 117
mitchpang 6:ad8a8c14b1f0 118 while(1) {
mitchpang 6:ad8a8c14b1f0 119 led1 = led2 = led4 = 1;
mitchpang 6:ad8a8c14b1f0 120 wait(0.5);
mitchpang 6:ad8a8c14b1f0 121 led1 = led2 = led4 = 0;
mitchpang 6:ad8a8c14b1f0 122 }
mitchpang 6:ad8a8c14b1f0 123 }
mitchpang 6:ad8a8c14b1f0 124 sensorModule_cs = 1; //deselect sensor module
mitchpang 6:ad8a8c14b1f0 125 fprintf(fp, "SPI communication to sensor module achieved\n");
mitchpang 6:ad8a8c14b1f0 126 */
mitchpang 0:bb537a3c7f2a 127
mitchpang 0:bb537a3c7f2a 128 //Calibrate the ADC
mitchpang 6:ad8a8c14b1f0 129 //Done by averaging 100 samples of adc
mitchpang 2:819397348cd0 130 for(int i = 0; i<100;++i){
mitchpang 2:819397348cd0 131 og1_calibration += og1*3.3;
mitchpang 2:819397348cd0 132 og2_calibration += og2*3.3;
mitchpang 2:819397348cd0 133 }
mitchpang 2:819397348cd0 134 og1_calibration = og1_calibration/100;
mitchpang 2:819397348cd0 135 og2_calibration = og2_calibration/100;
mitchpang 6:ad8a8c14b1f0 136 printf("og1_calibration value: %f\r\n",og1_calibration);
mitchpang 6:ad8a8c14b1f0 137 printf("og2_calibration value: %f\r\n",og2_calibration);
mitchpang 6:ad8a8c14b1f0 138 fprintf(fp,"og1_calibration value: %f\r\n",og1_calibration);
mitchpang 6:ad8a8c14b1f0 139 fprintf(fp,"og2_calibration value: %f\r\n",og2_calibration);
mitchpang 6:ad8a8c14b1f0 140 fprintf(fp,"Initialization Complete\r\n");
mitchpang 6:ad8a8c14b1f0 141 fprintf(fp,"Regrind#\tTimeSeen(us)\tVelocity(m/s)\tAcceleration(m/s2)\tdivert?\r\n");
mitchpang 6:ad8a8c14b1f0 142 fclose(fp);
mitchpang 2:819397348cd0 143 wait(3);
mitchpang 2:819397348cd0 144
mitchpang 6:ad8a8c14b1f0 145 //divertParticle.rise(&divert);
mitchpang 0:bb537a3c7f2a 146
mitchpang 0:bb537a3c7f2a 147 while(1) {
mitchpang 0:bb537a3c7f2a 148 //Sample ADCs
mitchpang 2:819397348cd0 149 og1_adc = og1.read()*3.3;
mitchpang 2:819397348cd0 150 og2_adc = og2.read()*3.3;
mitchpang 3:597f8492fee2 151 //og3_adc = og3.read()*3.3;
mitchpang 2:819397348cd0 152 //wait(0.01);
mitchpang 5:893aa74ff5aa 153 //pc.printf("og1: %f og2: %f \n\r",og1_adc, og2_adc);
mitchpang 5:893aa74ff5aa 154
mitchpang 2:819397348cd0 155 if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
mitchpang 0:bb537a3c7f2a 156 og1Oneshot = 1;
mitchpang 3:597f8492fee2 157 divertParticle.rise(&divert);
mitchpang 2:819397348cd0 158 //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
mitchpang 5:893aa74ff5aa 159 led1 = 1;
mitchpang 2:819397348cd0 160
mitchpang 0:bb537a3c7f2a 161 //Create Regrind
mitchpang 6:ad8a8c14b1f0 162 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);
mitchpang 6:ad8a8c14b1f0 163 //Send Regrind Position & Time Data
mitchpang 6:ad8a8c14b1f0 164 //sensorModule_cs = 0; //Select sensor module
mitchpang 6:ad8a8c14b1f0 165 //sensorModule.write((og1Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x0);//Tell Sensor PHD is about to send time data.
mitchpang 6:ad8a8c14b1f0 166 //sensorModule.write(regrindArray[og1Ndx%REGRIND_ARRAY_SIZE].velocity); //Write velocity data to sensor.
mitchpang 6:ad8a8c14b1f0 167 //sensorModule_cs = 1; //Deselect sensor module
mitchpang 2:819397348cd0 168 //wait(1);
mitchpang 0:bb537a3c7f2a 169 } //if(og1...)
mitchpang 2:819397348cd0 170 else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 171 og1Oneshot = 0;
mitchpang 0:bb537a3c7f2a 172
mitchpang 0:bb537a3c7f2a 173 }//else if(og1 ...)
mitchpang 0:bb537a3c7f2a 174
mitchpang 2:819397348cd0 175 if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){
mitchpang 0:bb537a3c7f2a 176 og2Oneshot = 1;
mitchpang 3:597f8492fee2 177 divertParticle.rise(NULL);
mitchpang 0:bb537a3c7f2a 178 regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
mitchpang 5:893aa74ff5aa 179 led2 = 1;
mitchpang 6:ad8a8c14b1f0 180
mitchpang 6:ad8a8c14b1f0 181 //Send Regrind Velocity Data
mitchpang 6:ad8a8c14b1f0 182 //sensorModule_cs = 0; //Select sensor module
mitchpang 6:ad8a8c14b1f0 183 //sensorModule.write((og2Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x1);//Tell Sensor PHD is about to send velocity data.
mitchpang 6:ad8a8c14b1f0 184 //sensorModule.write(regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].velocity); //Write velocity data to sensor.
mitchpang 6:ad8a8c14b1f0 185 //sensorModule_cs = 1; //Deselect sensor module
mitchpang 6:ad8a8c14b1f0 186
mitchpang 6:ad8a8c14b1f0 187 wait_ms(45);//Give time for sensor to collect data
mitchpang 6:ad8a8c14b1f0 188
mitchpang 6:ad8a8c14b1f0 189 //Get regrind divert data
mitchpang 6:ad8a8c14b1f0 190 //sensorModule_cs = 0; //Select sensor module
mitchpang 6:ad8a8c14b1f0 191 //sensorModule.write((og2Ndx%REGRIND_ARRAY_SIZE & 0x3F << 2) | 0x3);//Tell Sensor we need divert data.
mitchpang 6:ad8a8c14b1f0 192 //regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert = sensorModule.write(0x00000000);
mitchpang 6:ad8a8c14b1f0 193 //sensorModule_cs = 1; //Deselect sensor module
mitchpang 6:ad8a8c14b1f0 194
mitchpang 6:ad8a8c14b1f0 195 //Record data to SD card
mitchpang 6:ad8a8c14b1f0 196 FILE *fp = fopen("/sd/PHD/log.txt", "a");
mitchpang 6:ad8a8c14b1f0 197 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);
mitchpang 6:ad8a8c14b1f0 198 fclose(fp);
mitchpang 6:ad8a8c14b1f0 199
mitchpang 6:ad8a8c14b1f0 200 if(regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].divert == 1){
mitchpang 2:819397348cd0 201 solenoid = 1; //actuate solenoid if red
mitchpang 5:893aa74ff5aa 202 led4 = 1;
mitchpang 2:819397348cd0 203 }
mitchpang 2:819397348cd0 204 else {
mitchpang 5:893aa74ff5aa 205 //wait_ms(90);
mitchpang 2:819397348cd0 206 solenoid = 0; //Dont actuate if not red.
mitchpang 5:893aa74ff5aa 207 led4 = 0;
mitchpang 2:819397348cd0 208 }
mitchpang 6:ad8a8c14b1f0 209 regrindArray[og2Ndx].clearRegrind(); //reset divert flag
mitchpang 0:bb537a3c7f2a 210 }//if(og2..)
mitchpang 2:819397348cd0 211 else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 212 og2Oneshot = 0;
mitchpang 0:bb537a3c7f2a 213
mitchpang 0:bb537a3c7f2a 214 }//else if(og2 ...)
mitchpang 3:597f8492fee2 215 /*
mitchpang 2:819397348cd0 216 if((og3_adc == 0) && (og3Oneshot != 1)){
mitchpang 0:bb537a3c7f2a 217 og3Oneshot = 1;
mitchpang 0:bb537a3c7f2a 218 led4 = 1;
mitchpang 0:bb537a3c7f2a 219 regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
mitchpang 0:bb537a3c7f2a 220 if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid.
mitchpang 0:bb537a3c7f2a 221 solenoid = 1;
mitchpang 0:bb537a3c7f2a 222 }
mitchpang 0:bb537a3c7f2a 223 }//if(og3..)
mitchpang 2:819397348cd0 224 else if(og3_adc == 1){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 225 og3Oneshot = 0;
mitchpang 0:bb537a3c7f2a 226
mitchpang 0:bb537a3c7f2a 227 }//else if(og3 ...)
mitchpang 3:597f8492fee2 228 */
mitchpang 6:ad8a8c14b1f0 229
mitchpang 0:bb537a3c7f2a 230 //Check on 1pps clock
mitchpang 0:bb537a3c7f2a 231 if((totalT.read_us() % 2000000) < 1000000){
mitchpang 6:ad8a8c14b1f0 232 led3 = 1;
mitchpang 0:bb537a3c7f2a 233 onePPS_out = 1;
mitchpang 0:bb537a3c7f2a 234 }
mitchpang 0:bb537a3c7f2a 235 else {//timer is in off cycle
mitchpang 6:ad8a8c14b1f0 236 led3 = 0;
mitchpang 0:bb537a3c7f2a 237 onePPS_out = 0;
mitchpang 6:ad8a8c14b1f0 238 }
mitchpang 0:bb537a3c7f2a 239
mitchpang 0:bb537a3c7f2a 240 //Check if data writing flag is set - if so, write to SD card
mitchpang 0:bb537a3c7f2a 241
mitchpang 0:bb537a3c7f2a 242 //Adjust PWM as necessary
mitchpang 3:597f8492fee2 243 augerMotors.Dir1 = reverseMotor1pb;
mitchpang 3:597f8492fee2 244 augerMotors.setpwm1pulsewidth(topMotorAdjuster.read());
mitchpang 3:597f8492fee2 245 augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read());
mitchpang 2:819397348cd0 246 //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
mitchpang 0:bb537a3c7f2a 247 } //while(1)
mitchpang 0:bb537a3c7f2a 248 totalT.stop();
mitchpang 6:ad8a8c14b1f0 249 //fclose(fp);
mitchpang 0:bb537a3c7f2a 250 }// int main()