main.cpp

00001 #include "mbed.h"
00002 #include "SWO.h"
00003 #include "PID.h"
00004 
00005 #define RATE 5 //interval PID calculation performed every 5th milliseconds.
00006 
00007 
00008 
00009 DigitalOut LED_GREEN(PA_4);
00010 DigitalOut LED_RED(PA_5);
00011 DigitalOut LED_ENABLE(PB_5);
00012 
00013 //optical sensors
00014 DigitalIn SCANNER_INPUT(PC_10);
00015 DigitalIn SCANNER_OUTPUT(PC_1);
00016 DigitalIn SWITCH_INPUT(PC_2);
00017 DigitalIn REJECT_INPUT(PA_7);
00018 DigitalIn REJECT_OUTPUT(PA_6);
00019 DigitalIn STORAGE_INPUT(PC_3);
00020 
00021 int opticalSensors[6] = {0};    //list to store the values from optical sensors
00022 int lastValueOpticalSensors[6] = {0};    //list to store the previous values from optical sensors
00023 float sensorNoteRunningTimes[10] = {0};  //The times (in us) when note hit the sensors are stored in this array
00024 int pulseCountBetweenSensors[10] = {0};  //The counted number encoder pulses between sensors
00025 int noteInSystem = 0;
00026 long lastPulseTime = 0;
00027 long maxTimeBetweenPulses = 0;
00028 long minTimeBetweenPulses = 10000000;
00029 long timeBetweenPulses = 0;
00030 double currentSpeed = 0; //speed in mm/ms
00031 long lastControl = 0;
00032 
00033 PID controller(4.0, 0.0, 0.0, RATE);
00034 
00035 
00036 int AtoB_dist = 357;    //distance from SCANNER_INPUT to SCANNER_OUTPUT     counted pulses: 372
00037 int BtoC_dist = 81;     //distance from SCANNER_OUTPUT to SWITCH_INPUT      counted pulses: 82
00038 int CtoD_dist = 201;    //distance from SWITCH_INPUT to REJECT_INPUT        counted pulses: 203
00039 int DtoE_dist = 158;    //distance from REJECT_INPUT to REJECT_OUTPUT       counted pulses: 156
00040 int Tot_dist = 797;     //distance from SCANNER_INPUT to REJECT_OUTPUT      counted pulses: 813
00041 
00042 int TenEURnoteLength = 127;
00043 int FiftyEURnoteLength = 140;
00044 int TwentyEURnoteLength = 120;
00045 
00046 //encoders
00047 InterruptIn MAIN_ENC(PA_8);
00048 //InterruptIn STORAGE_ENC(PA_15);
00049 //InterruptIn SWITCH_ENC(PB_4);
00050 //InterruptIn ROUTER_ENC(PB_6);
00051 
00052 
00053 //Motors
00054 PwmOut MAIN_PWM(PC_7);   //PWM to Main Drive
00055 DigitalOut MAIN_DIR(PC_0);   //Direction of main drive
00056 
00057 PwmOut SWITCH_PWM(PB_14);   //PWM to Switch
00058 DigitalOut SWITCH_DIR(PB_15);   //Direction of Switch
00059 
00060 PwmOut ROUTER_PWM(PC_8);   //PWM to Router
00061 DigitalOut ROUTER_DIR(PB_7);   //Direction of Router
00062 
00063 PwmOut STORAGE_PWM(PC_6);   //PWM to Storage
00064 DigitalOut STORAGE_DIR(PC_9);   //Direction of Storage
00065 
00066 
00067 
00068 uint32_t blinkTime_ms = 0;
00069 uint32_t reportTimer = 0;
00070 uint32_t pulses = 0;
00071 
00072 
00073 Timer t;
00074 //PID controller(1, 0, 0, RATE);    //Kc, Ti, Td, interval
00075 
00076 
00077 Serial pc(USBTX, USBRX);
00078 SWO_Channel swo("channel");
00079 
00080 void green_blink(void)
00081 {
00082     if(t.read_ms() - blinkTime_ms > 200) {
00083         LED_RED = 0;
00084         LED_GREEN = !LED_GREEN;
00085         blinkTime_ms = t.read_ms();
00086     }
00087 }
00088 void red_blink(void)
00089 {
00090     if(t.read_ms() - blinkTime_ms > 200) {
00091         LED_GREEN = 0;
00092         LED_RED = !LED_RED;
00093         blinkTime_ms = t.read_ms();
00094     }
00095 }
00096 
00097 void reportNoteThroughSensorLengths()
00098 {
00099 
00100     swo.printf("\n");
00101 
00102     //Time for note to travel through first sensor
00103     long time = (sensorNoteRunningTimes[1] - sensorNoteRunningTimes[0]); //us
00104     swo.printf("Time for note to pass 1th sensor (us): %d\r", time);
00105     //int length = TwentyEURnoteLength;
00106     int countedPulses = pulseCountBetweenSensors[1] - pulseCountBetweenSensors[0];
00107     swo.printf("  Counted pulses: %d\r\n", countedPulses);
00108 
00109     //Time for note to travel through second sensor
00110     time = (sensorNoteRunningTimes[3] - sensorNoteRunningTimes[2]); //us
00111     swo.printf("Time for note to pass 2th sensor (us): %d\r\n", time);
00112     countedPulses = pulseCountBetweenSensors[3] - pulseCountBetweenSensors[2];
00113     swo.printf("  Counted pulses: %d\r\n", countedPulses);
00114 
00115     //Time for note to travel through third sensor
00116     time = (sensorNoteRunningTimes[5] - sensorNoteRunningTimes[4]); //us
00117     swo.printf("Time for note to pass 3th sensor (us): %d\r\n", time);
00118     countedPulses = pulseCountBetweenSensors[5] - pulseCountBetweenSensors[4];
00119     swo.printf("  Counted pulses: %d\r\n", countedPulses);
00120 
00121     //Time for note to travel through fourth sensor
00122     time = (sensorNoteRunningTimes[7] - sensorNoteRunningTimes[6]); //us
00123     swo.printf("Time for note to pass 4th sensor (us): %d\r\n", time);
00124     countedPulses = pulseCountBetweenSensors[7] - pulseCountBetweenSensors[6];
00125     swo.printf("  Counted pulses: %d\r\n", countedPulses);
00126 
00127     //Time for note to travel through fifth sensor
00128     time = (sensorNoteRunningTimes[9] - sensorNoteRunningTimes[8]); //us
00129     swo.printf("Time for note to pass 5th sensor (us): %d\r\n", time);
00130     countedPulses = pulseCountBetweenSensors[9] - pulseCountBetweenSensors[8];
00131     swo.printf("  Counted pulses: %d\r\n", countedPulses);
00132 
00133     swo.printf("\n");
00134     swo.printf("\n");
00135 
00136 }
00137 
00138 void report(void)
00139 {
00140     //uint32_t now = t.read_ms();
00141 
00142     /*
00143     swo.printf("Optical sensor status: ");
00144     for(int j = 0; j <=5 ; j++) {
00145         swo.printf("%d ", opticalSensors[j]);
00146     }
00147     swo.printf("\n");
00148     */
00149 
00150     long time = (sensorNoteRunningTimes[2] - sensorNoteRunningTimes[0]) / 1000; //ms
00151 
00152     //swo.printf("Time between first two sensors (ms): ");
00153     //swo.printf("%d ", time);
00154     //swo.printf("\n");
00155 
00156     //V = S / T
00157     float V = 1000 * AtoB_dist / time; //(mm/s)
00158 
00159     swo.printf("Speed between first two sensors (mm/s): ");
00160     swo.printf("%f ", V);
00161     swo.printf("  Counted encoder pulses: ");
00162     uint32_t countedPulses = pulseCountBetweenSensors[2] - pulseCountBetweenSensors[0];
00163     swo.printf("%d ", countedPulses);
00164     swo.printf("  ->Encoder speed (mm/s): ");
00165     float countedSpeed = 1000 * countedPulses / time;
00166     swo.printf("%f ", countedSpeed);
00167     swo.printf("  ->Speed difference (percentage): ");
00168     double difference = (100 * V / countedSpeed) - 100;
00169     swo.printf("%f ", difference);
00170 
00171     swo.printf("\n");
00172 
00173     time = (sensorNoteRunningTimes[4] - sensorNoteRunningTimes[2]) / 1000; //ms
00174     V = 1000 * BtoC_dist / time; //(mm/s)
00175 
00176     swo.printf("Speed between second and third sensor (mm/s): ");
00177     swo.printf("%f ", V);
00178     swo.printf("  Counted encoder pulses: ");
00179     countedPulses = pulseCountBetweenSensors[4] - pulseCountBetweenSensors[2];
00180     swo.printf("%d ", countedPulses);
00181     swo.printf("  ->Encoder speed (mm/s): ");
00182     countedSpeed = 1000 * countedPulses / time;
00183     swo.printf("%f ", countedSpeed);
00184     swo.printf("  ->Speed difference (percentage): ");
00185     difference = (100 * V / countedSpeed) - 100;
00186     swo.printf("%f ", difference);
00187 
00188     swo.printf("\n");
00189 
00190     time = (sensorNoteRunningTimes[6] - sensorNoteRunningTimes[4]) / 1000; //ms
00191     V = 1000 * CtoD_dist / time; //(mm/s)
00192 
00193     swo.printf("Speed between third and fourth sensor (mm/s): ");
00194     swo.printf("%f ", V);
00195     swo.printf("  Counted encoder pulses: ");
00196     countedPulses = pulseCountBetweenSensors[6] - pulseCountBetweenSensors[4];
00197     swo.printf("%d ", countedPulses);
00198     swo.printf("  ->Encoder speed (mm/s): ");
00199     countedSpeed = 1000 * countedPulses / time;
00200     swo.printf("%f ", countedSpeed);
00201     swo.printf("  ->Speed difference (percentage): ");
00202     difference = (100 * V / countedSpeed) - 100;
00203     swo.printf("%f ", difference);
00204 
00205     swo.printf("\n");
00206 
00207     time = (sensorNoteRunningTimes[8] - sensorNoteRunningTimes[6]) / 1000; //ms
00208     V = 1000 * DtoE_dist / time; //(mm/s)
00209 
00210     swo.printf("Speed between fourth and fifth sensor (mm/s): ");
00211     swo.printf("%f ", V);
00212     swo.printf("  Counted encoder pulses: ");
00213     countedPulses = pulseCountBetweenSensors[8] - pulseCountBetweenSensors[6];
00214     swo.printf("%d ", countedPulses);
00215     swo.printf("  ->Encoder speed (mm/s): ");
00216     countedSpeed = 1000 * countedPulses / time;
00217     swo.printf("%f ", countedSpeed);
00218     swo.printf("  ->Speed difference (percentage): ");
00219     difference = (100 * V / countedSpeed) - 100;
00220     swo.printf("%f ", difference);
00221 
00222     swo.printf("\n");
00223     swo.printf("\n");
00224 
00225     time = (sensorNoteRunningTimes[8] - sensorNoteRunningTimes[0]) / 1000; //ms
00226     V = 1000 * Tot_dist / time; //(mm/s)
00227 
00228     swo.printf("Total mean speed between first and last sensor (mm/s): ");
00229     swo.printf("%f ", V);
00230     swo.printf("  Total counted encoder pulses: ");
00231     countedPulses = pulseCountBetweenSensors[8] - pulseCountBetweenSensors[0];
00232     swo.printf("%d ", countedPulses);
00233     swo.printf("  ->Encoder speed (mm/s): ");
00234     countedSpeed = 1000 * countedPulses / time;
00235     swo.printf("%f ", countedSpeed);
00236     swo.printf("  ->Total speed difference (percentage): ");
00237     difference = (100 * V / countedSpeed) - 100;
00238     swo.printf("%f ", difference);
00239 
00240     swo.printf("\n");
00241     swo.printf("\n");
00242 
00243     swo.printf("Maximum time between pulses (us): %d\r\n", maxTimeBetweenPulses);
00244     swo.printf("Minimum time between pulses (us): %d\r\n", minTimeBetweenPulses);
00245 
00246     swo.printf("\n");
00247     swo.printf("Filtered speed value to PID (mm/s): ");
00248     swo.printf("%f ", currentSpeed);
00249     
00250 
00251     swo.printf("\n");
00252     swo.printf("\n");
00253 
00254 
00255     reportNoteThroughSensorLengths();
00256     
00257     maxTimeBetweenPulses = 0;
00258     minTimeBetweenPulses = 10000000;
00259     pulses = 0;
00260 
00261     reportTimer = t.read_ms();
00262 }
00263 
00264 
00265 
00266 void encTick(void)
00267 {
00268     long now = t.read_us();
00269     timeBetweenPulses = now - lastPulseTime;
00270 
00271     if(noteInSystem) {
00272         if(timeBetweenPulses > maxTimeBetweenPulses) maxTimeBetweenPulses = timeBetweenPulses;  //storing maximum time between pulses
00273         if(timeBetweenPulses < minTimeBetweenPulses) minTimeBetweenPulses = timeBetweenPulses;  //storing minimum time between pulses
00274     }
00275 
00276     lastPulseTime = now;
00277 
00278     pulses++;
00279 
00280     //filtered 10%
00281     currentSpeed = 0.05 * (1000000/timeBetweenPulses) + 0.95 * currentSpeed;  //mm/s
00282     //currentSpeed = 1000000/timeBetweenPulses;  //mm/s
00283 }
00284 
00285 void checkOpticalSensors(void)
00286 {
00287 
00288     for(int i=0; i<=5; i++) {
00289         opticalSensors[i]=0;
00290     }
00291 
00292     if(SCANNER_INPUT) {
00293         opticalSensors[0]=1;
00294         if(lastValueOpticalSensors[0] == 0) {
00295             sensorNoteRunningTimes[0] = t.read_us();    //leading edge of note detected on first optical sensor - storing time
00296             noteInSystem = 1;   //note entered the system
00297             pulseCountBetweenSensors[0] = pulses;
00298         }
00299     } else {
00300         if(lastValueOpticalSensors[0] == 1) {
00301             sensorNoteRunningTimes[1] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
00302             pulseCountBetweenSensors[1] = pulses;
00303         }
00304     }
00305 
00306     if(SCANNER_OUTPUT) {
00307         opticalSensors[1]=1;
00308         if(lastValueOpticalSensors[1] == 0) {
00309             sensorNoteRunningTimes[2] = t.read_us();    //note detected on second optical sensor - storing time
00310             pulseCountBetweenSensors[2] = pulses;
00311         }
00312     } else {
00313         if(lastValueOpticalSensors[1] == 1) {
00314             sensorNoteRunningTimes[3] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
00315             pulseCountBetweenSensors[3] = pulses;
00316         }
00317     }
00318 
00319     if(SWITCH_INPUT) {
00320         opticalSensors[2]=1;
00321         if(lastValueOpticalSensors[2] == 0) {
00322             sensorNoteRunningTimes[4] = t.read_us();    //note detected on second optical sensor - storing time
00323             pulseCountBetweenSensors[4] = pulses;
00324         }
00325     } else {
00326         if(lastValueOpticalSensors[2] == 1) {
00327             sensorNoteRunningTimes[5] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
00328             pulseCountBetweenSensors[5] = pulses;
00329         }
00330     }
00331 
00332     if(REJECT_INPUT) {
00333         opticalSensors[3]=1;
00334         if(lastValueOpticalSensors[3] == 0) {
00335             sensorNoteRunningTimes[6] = t.read_us();    //note detected on second optical sensor - storing time
00336             pulseCountBetweenSensors[6] = pulses;
00337         }
00338     } else {
00339         if(lastValueOpticalSensors[3] == 1) {
00340             sensorNoteRunningTimes[7] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
00341             pulseCountBetweenSensors[7] = pulses;
00342         }
00343     }
00344 
00345     if(REJECT_OUTPUT) {
00346         opticalSensors[4]=1;
00347         if(lastValueOpticalSensors[4] == 0) {
00348             sensorNoteRunningTimes[8] = t.read_us();    //note detected on second optical sensor - storing time
00349             pulseCountBetweenSensors[8] = pulses;
00350         }
00351     } else {
00352         if(lastValueOpticalSensors[4] == 1) {
00353             sensorNoteRunningTimes[9] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
00354             pulseCountBetweenSensors[9] = pulses;
00355             noteInSystem = 0;   //note left the system
00356             report();
00357         }
00358     }
00359 
00360     if(STORAGE_INPUT) opticalSensors[5]=1;
00361 
00362     //storing sensor state in previous value array
00363     for(int i=0; i<=5; i++) {
00364         lastValueOpticalSensors[i] = opticalSensors[i];
00365     }
00366 
00367 }
00368 
00369 void tunePID()
00370 {
00371 
00372     swo.printf("setting 60percent output %\n");
00373     MAIN_PWM.write(0.6f);
00374     swo.printf("waiting for speedup... %\n");
00375     wait(2);    //waiting 2 sec
00376     swo.printf("Current speed (mm/s): %f\n", currentSpeed);
00377     double beforeSpeed = currentSpeed;
00378     swo.printf("setting 70percent output %\n");
00379     MAIN_PWM.write(0.7f);   //10% more output
00380     swo.printf("waiting for speedup... %\n");
00381     wait(2);    //waiting 2 sec
00382     swo.printf("Current speed (mm/s): %f\n", currentSpeed);
00383     double deltaSpeed = currentSpeed - beforeSpeed;
00384     swo.printf("delta Speed (mm/s): %f\n", deltaSpeed);
00385     double K = deltaSpeed / 10;
00386     swo.printf("PID K-factor (deltaSpeed/10): %f\n", K);
00387 
00388     controller.setTunings(K/2, 0, 0); //K/2?
00389 
00390     controller.setInputLimits(0.0, 600);
00391 
00392     //Pwm output from 0.0 to 1.0
00393     controller.setOutputLimits(0.0, 1.0);
00394 
00395     controller.setMode(AUTO_MODE);
00396 
00397     controller.setSetPoint(600);
00398 
00399     swo.printf("\n");
00400     swo.printf("\n");
00401     swo.printf("\n");
00402 
00403 }
00404 
00405 
00406 int main()
00407 {
00408     LED_ENABLE = 1; //turning leds on
00409     LED_GREEN = 0;
00410     LED_RED = 0;
00411 
00412     //Setting motors off
00413     MAIN_PWM.write(0.00f);
00414     SWITCH_PWM.write(0.00f);
00415     ROUTER_PWM.write(0.00f);
00416     STORAGE_PWM.write(0.00f);
00417     MAIN_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
00418     SWITCH_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
00419     ROUTER_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
00420     STORAGE_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
00421 
00422 
00423     swo.printf("STARTED %\n");
00424     swo.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);    //SYTEM CLOCK 72MHz
00425     MAIN_ENC.rise(&encTick);  // attach the address of the encTick function to the rising edge
00426 
00427     t.start();
00428 
00429     //MAIN_PWM.write(0.7f); //40% works 0.4f
00430 
00431     tunePID();
00432 
00433 
00434     while(1) {
00435 
00436 
00437         checkOpticalSensors();  //updating opticalSensors array
00438 
00439         //if(t.read_ms() - reportTimer > 1000 && noteInSystem == 0) report();  //reporting every second
00440 
00441         if(t.read_ms() -  lastControl > RATE) {
00442             //Update the process variable.
00443             controller.setProcessValue(currentSpeed);
00444             //Set the new output.
00445             double output = controller.compute();
00446             MAIN_PWM.write(output);
00447             //swo.printf("sent to controller: %f\n", output);
00448             lastControl = t.read_ms();
00449         }
00450 
00451     }
00452 
00453 
00454 }
00455 
00456