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 Mitchell Pang

Committer:
mitchpang
Date:
Thu Dec 03 06:36:21 2015 +0000
Revision:
3:597f8492fee2
Parent:
2:819397348cd0
Child:
5:893aa74ff5aa
Changed H-Bridge

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 0:bb537a3c7f2a 5
mitchpang 0:bb537a3c7f2a 6 #define OG1_TO_OG2_DIST 1
mitchpang 0:bb537a3c7f2a 7 #define OG1_TO_OG3_DIST 2
mitchpang 2:819397348cd0 8 #define SOLENOID_ON_DELAY 0.5
mitchpang 0:bb537a3c7f2a 9 #define SOLENOID_OFF_DELAY 0.0
mitchpang 0:bb537a3c7f2a 10 #define LED_ON_DELAY 0.5
mitchpang 0:bb537a3c7f2a 11 #define LED_OFF_DELAY 0.0
mitchpang 0:bb537a3c7f2a 12 #define REGRIND_ARRAY_SIZE 1
mitchpang 2:819397348cd0 13 #define THRESHOLD 0.015
mitchpang 2:819397348cd0 14 #define RETURN_THRESHOLD 0.01
mitchpang 0:bb537a3c7f2a 15
mitchpang 0:bb537a3c7f2a 16 DigitalOut led1(LED1,0); //Used as 1pps out indicator
mitchpang 0:bb537a3c7f2a 17 Solenoid led2(LED2, LED_ON_DELAY, LED_OFF_DELAY);
mitchpang 0:bb537a3c7f2a 18 Solenoid led3(LED3, LED_ON_DELAY, LED_OFF_DELAY);
mitchpang 0:bb537a3c7f2a 19 Solenoid led4(LED4, LED_ON_DELAY, LED_OFF_DELAY);
mitchpang 0:bb537a3c7f2a 20 DigitalOut onePPS_out(p29);
mitchpang 0:bb537a3c7f2a 21 Solenoid solenoid(p30, SOLENOID_ON_DELAY, SOLENOID_OFF_DELAY); //Solenoid(PinName pin, float ondelay, float offdelay)
mitchpang 3:597f8492fee2 22 RioRandHBridge augerMotors(p21, p25, p22, p23); //RioRandHBridge( PinName pinPwm1, PinName pinDir1, PinName pinPwm2, PinName pinDir2);
mitchpang 3:597f8492fee2 23 DigitalIn reverseMotor1pb(p26);
mitchpang 3:597f8492fee2 24 DigitalIn reverseMotor2pb(p24);
mitchpang 2:819397348cd0 25 AnalogIn topMotorAdjuster(p19);
mitchpang 3:597f8492fee2 26 DigitalOut bottomMotorAdjuster(p16,0);
mitchpang 2:819397348cd0 27 DigitalOut unused1(p17,0);
mitchpang 2:819397348cd0 28 AnalogIn og1(p15);
mitchpang 2:819397348cd0 29 AnalogIn og2(p20);
mitchpang 3:597f8492fee2 30 DigitalOut og3(p18,0);
mitchpang 2:819397348cd0 31 InterruptIn divertParticle(p5);
mitchpang 0:bb537a3c7f2a 32 Timer totalT;
mitchpang 2:819397348cd0 33 DigitalOut startColor(p6);
mitchpang 2:819397348cd0 34 //DigitalIn redColor(p5);
mitchpang 2:819397348cd0 35 DigitalIn blueColor(p7);
mitchpang 2:819397348cd0 36 DigitalIn greenColor(p8);
mitchpang 0:bb537a3c7f2a 37 //Ticker 1pps;
mitchpang 0:bb537a3c7f2a 38
mitchpang 0:bb537a3c7f2a 39 Serial pc(USBTX,USBRX); //used for debugging
mitchpang 0:bb537a3c7f2a 40
mitchpang 2:819397348cd0 41 float og1Threshold = 0.3;
mitchpang 0:bb537a3c7f2a 42 float og2Threshold = 0;
mitchpang 0:bb537a3c7f2a 43 float og3Threshold = 0;
mitchpang 0:bb537a3c7f2a 44 int og1Oneshot = 0;
mitchpang 0:bb537a3c7f2a 45 int og2Oneshot = 0;
mitchpang 0:bb537a3c7f2a 46 int og3Oneshot = 0;
mitchpang 0:bb537a3c7f2a 47 float og1_adc = 0;
mitchpang 0:bb537a3c7f2a 48 float og2_adc = 0;
mitchpang 0:bb537a3c7f2a 49 float og3_adc = 0;
mitchpang 0:bb537a3c7f2a 50 int og1Ndx = 0;
mitchpang 0:bb537a3c7f2a 51 int og2Ndx = 0;
mitchpang 0:bb537a3c7f2a 52 int og3Ndx = 0;
mitchpang 0:bb537a3c7f2a 53 float og1_min = 0;
mitchpang 2:819397348cd0 54 float og1_max = 2.7;
mitchpang 0:bb537a3c7f2a 55 float og2_min = 0;
mitchpang 0:bb537a3c7f2a 56 float og2_max = 0;
mitchpang 0:bb537a3c7f2a 57 float og3_min = 0;
mitchpang 0:bb537a3c7f2a 58 float og3_max = 0;
mitchpang 2:819397348cd0 59 float og1_calibration = 0;
mitchpang 2:819397348cd0 60 float og2_calibration = 0;
mitchpang 2:819397348cd0 61 float og3_calibration = 0;
mitchpang 0:bb537a3c7f2a 62
mitchpang 0:bb537a3c7f2a 63 Regrind regrindArray[REGRIND_ARRAY_SIZE];
mitchpang 0:bb537a3c7f2a 64 /*
mitchpang 0:bb537a3c7f2a 65 void flip(){
mitchpang 0:bb537a3c7f2a 66 led1 = !led1;
mitchpang 0:bb537a3c7f2a 67 1pps_out = !1pps_out;
mitchpang 0:bb537a3c7f2a 68 }
mitchpang 0:bb537a3c7f2a 69 */
mitchpang 2:819397348cd0 70 void divert(){
mitchpang 2:819397348cd0 71 regrindArray[og2Ndx].divert = 1;
mitchpang 2:819397348cd0 72 }
mitchpang 3:597f8492fee2 73 void reverseMotor1(){
mitchpang 3:597f8492fee2 74 augerMotors.Dir1 = !augerMotors.Dir1;
mitchpang 3:597f8492fee2 75 }
mitchpang 3:597f8492fee2 76 void reverseMotor2(){
mitchpang 3:597f8492fee2 77 augerMotors.Dir2 = !augerMotors.Dir2;
mitchpang 3:597f8492fee2 78 }
mitchpang 0:bb537a3c7f2a 79
mitchpang 0:bb537a3c7f2a 80 int main() {
mitchpang 0:bb537a3c7f2a 81 //Start Clock
mitchpang 0:bb537a3c7f2a 82 totalT.start();
mitchpang 0:bb537a3c7f2a 83 //Setup Information
mitchpang 0:bb537a3c7f2a 84 /*
mitchpang 0:bb537a3c7f2a 85 led1 = 0;
mitchpang 0:bb537a3c7f2a 86 1pps_out = 0;
mitchpang 0:bb537a3c7f2a 87 1pps.attach(&flip, 1.0);
mitchpang 0:bb537a3c7f2a 88 */
mitchpang 0:bb537a3c7f2a 89 //Setup motors
mitchpang 3:597f8492fee2 90 reverseMotor1pb.mode( PullUp );
mitchpang 3:597f8492fee2 91 reverseMotor2pb.mode( PullUp );
mitchpang 3:597f8492fee2 92 //reverseMotor1pb.attach_asserted( &reverseMotor1 );
mitchpang 3:597f8492fee2 93 //reverseMotor2pb.attach_asserted (&reverseMotor2);
mitchpang 3:597f8492fee2 94 augerMotors.setpwm1pulsewidth(0.0);
mitchpang 3:597f8492fee2 95 augerMotors.setpwm2pulsewidth(0.0);
mitchpang 3:597f8492fee2 96 augerMotors.motor1_ccw();
mitchpang 3:597f8492fee2 97 augerMotors.motor2_ccw();
mitchpang 0:bb537a3c7f2a 98
mitchpang 0:bb537a3c7f2a 99 //Spin up Motors until fluctuation of 10% seen
mitchpang 0:bb537a3c7f2a 100
mitchpang 0:bb537a3c7f2a 101
mitchpang 0:bb537a3c7f2a 102 //Calibrate the ADC
mitchpang 0:bb537a3c7f2a 103 //Done by spinning augers and measuring min and max then setting threshold as 0.9*(max-min) + min
mitchpang 2:819397348cd0 104 for(int i = 0; i<100;++i){
mitchpang 2:819397348cd0 105 og1_calibration += og1*3.3;
mitchpang 2:819397348cd0 106 og2_calibration += og2*3.3;
mitchpang 2:819397348cd0 107 }
mitchpang 2:819397348cd0 108 og1_calibration = og1_calibration/100;
mitchpang 2:819397348cd0 109 og2_calibration = og2_calibration/100;
mitchpang 2:819397348cd0 110 printf("og1_calibration value: %f\n\r",og1_calibration);
mitchpang 2:819397348cd0 111 printf("og2_calibration value: %f\n\r",og2_calibration);
mitchpang 2:819397348cd0 112 wait(3);
mitchpang 2:819397348cd0 113
mitchpang 2:819397348cd0 114 divertParticle.rise(&divert);
mitchpang 0:bb537a3c7f2a 115
mitchpang 0:bb537a3c7f2a 116 while(1) {
mitchpang 2:819397348cd0 117 /*
mitchpang 2:819397348cd0 118 if(redColor == 1){
mitchpang 2:819397348cd0 119 solenoid = 1; //actuate solenoid if red
mitchpang 2:819397348cd0 120 led1=1;
mitchpang 2:819397348cd0 121 }
mitchpang 2:819397348cd0 122 else {
mitchpang 2:819397348cd0 123 solenoid = 0; //actuate solenoid no matter what.
mitchpang 2:819397348cd0 124 led1=0;
mitchpang 2:819397348cd0 125 }
mitchpang 2:819397348cd0 126 */
mitchpang 0:bb537a3c7f2a 127 //Sample ADCs
mitchpang 2:819397348cd0 128 og1_adc = og1.read()*3.3;
mitchpang 2:819397348cd0 129 og2_adc = og2.read()*3.3;
mitchpang 3:597f8492fee2 130 //og3_adc = og3.read()*3.3;
mitchpang 2:819397348cd0 131 //wait(0.01);
mitchpang 2:819397348cd0 132 //pc.printf("og1: %f og2: %f og3: %f\n\r",og1_adc, og2_adc, og3_adc);
mitchpang 0:bb537a3c7f2a 133
mitchpang 2:819397348cd0 134 if((og1_calibration - og1_adc > THRESHOLD) && (og1Oneshot != 1)){ //Something passed through og1
mitchpang 0:bb537a3c7f2a 135 og1Oneshot = 1;
mitchpang 3:597f8492fee2 136 divertParticle.rise(&divert);
mitchpang 2:819397348cd0 137 //pc.printf("Regrind seen at OG 1 : %fV\n\r", og1_adc);
mitchpang 0:bb537a3c7f2a 138 led2 = 1;
mitchpang 2:819397348cd0 139
mitchpang 0:bb537a3c7f2a 140 //Create Regrind
mitchpang 0:bb537a3c7f2a 141 regrindArray[og1Ndx%REGRIND_ARRAY_SIZE] = Regrind(totalT.read_us(), 1, 0, 0, 0, 0);
mitchpang 2:819397348cd0 142 startColor = 1;
mitchpang 2:819397348cd0 143 //wait(1);
mitchpang 0:bb537a3c7f2a 144 } //if(og1...)
mitchpang 2:819397348cd0 145 else if(og1_calibration - og1_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 146 og1Oneshot = 0;
mitchpang 0:bb537a3c7f2a 147
mitchpang 0:bb537a3c7f2a 148 }//else if(og1 ...)
mitchpang 0:bb537a3c7f2a 149
mitchpang 2:819397348cd0 150 if((og2_calibration - og2_adc > THRESHOLD) && (og2Oneshot != 1)){
mitchpang 0:bb537a3c7f2a 151 og2Oneshot = 1;
mitchpang 3:597f8492fee2 152 divertParticle.rise(NULL);
mitchpang 0:bb537a3c7f2a 153 regrindArray[og2Ndx%REGRIND_ARRAY_SIZE].setVelocity(OG1_TO_OG2_DIST);
mitchpang 0:bb537a3c7f2a 154 led3 = 1;
mitchpang 2:819397348cd0 155 if(regrindArray[og2Ndx].divert == 1){
mitchpang 2:819397348cd0 156 solenoid = 1; //actuate solenoid if red
mitchpang 2:819397348cd0 157 }
mitchpang 2:819397348cd0 158 else {
mitchpang 2:819397348cd0 159 solenoid = 0; //Dont actuate if not red.
mitchpang 2:819397348cd0 160 }
mitchpang 0:bb537a3c7f2a 161 }//if(og2..)
mitchpang 2:819397348cd0 162 else if(og2_calibration - og2_adc < RETURN_THRESHOLD){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 163 og2Oneshot = 0;
mitchpang 0:bb537a3c7f2a 164
mitchpang 0:bb537a3c7f2a 165 }//else if(og2 ...)
mitchpang 3:597f8492fee2 166 /*
mitchpang 2:819397348cd0 167 if((og3_adc == 0) && (og3Oneshot != 1)){
mitchpang 0:bb537a3c7f2a 168 og3Oneshot = 1;
mitchpang 0:bb537a3c7f2a 169 led4 = 1;
mitchpang 0:bb537a3c7f2a 170 regrindArray[og3Ndx].setAcceleration(OG1_TO_OG3_DIST);
mitchpang 0:bb537a3c7f2a 171 if(regrindArray[og3Ndx].divert == 1){//Regrind has been selected to be diverted. Turn on solenoid.
mitchpang 0:bb537a3c7f2a 172 solenoid = 1;
mitchpang 0:bb537a3c7f2a 173 }
mitchpang 0:bb537a3c7f2a 174 }//if(og3..)
mitchpang 2:819397348cd0 175 else if(og3_adc == 1){ //Regrind has passed ok to reset og
mitchpang 0:bb537a3c7f2a 176 og3Oneshot = 0;
mitchpang 0:bb537a3c7f2a 177
mitchpang 0:bb537a3c7f2a 178 }//else if(og3 ...)
mitchpang 3:597f8492fee2 179 */
mitchpang 0:bb537a3c7f2a 180 //Check on 1pps clock
mitchpang 0:bb537a3c7f2a 181 if((totalT.read_us() % 2000000) < 1000000){
mitchpang 0:bb537a3c7f2a 182 led1 = 1;
mitchpang 0:bb537a3c7f2a 183 onePPS_out = 1;
mitchpang 0:bb537a3c7f2a 184 }
mitchpang 0:bb537a3c7f2a 185 else {//timer is in off cycle
mitchpang 0:bb537a3c7f2a 186 led1 = 0;
mitchpang 0:bb537a3c7f2a 187 onePPS_out = 0;
mitchpang 0:bb537a3c7f2a 188 }
mitchpang 0:bb537a3c7f2a 189
mitchpang 0:bb537a3c7f2a 190 //Check if data writing flag is set - if so, write to SD card
mitchpang 0:bb537a3c7f2a 191
mitchpang 0:bb537a3c7f2a 192 //Adjust PWM as necessary
mitchpang 3:597f8492fee2 193 augerMotors.Dir1 = reverseMotor1pb;
mitchpang 3:597f8492fee2 194 augerMotors.setpwm1pulsewidth(topMotorAdjuster.read());
mitchpang 3:597f8492fee2 195 augerMotors.setpwm2pulsewidth(bottomMotorAdjuster.read());
mitchpang 2:819397348cd0 196 //pc.printf("top: %f bottom: %f\n\r",topMotorAdjuster.read(),bottomMotorAdjuster.read());
mitchpang 0:bb537a3c7f2a 197 } //while(1)
mitchpang 0:bb537a3c7f2a 198 totalT.stop();
mitchpang 0:bb537a3c7f2a 199 }// int main()