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@2:819397348cd0, 2015-12-02 (annotated)

Committer:
mitchpang
Date:
Wed Dec 02 15:43:38 2015 +0000
Revision:
2:819397348cd0
Parent:
0:bb537a3c7f2a
Child:
3:597f8492fee2
Changed working structure for og1 and og2

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