Diff: main.cpp

Revision:

0:fc81857d8067

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 30 10:22:36 2018 +0000
@@ -0,0 +1,456 @@
+#include "mbed.h"
+#include "SWO.h"
+#include "PID.h"
+
+#define RATE 5 //interval PID calculation performed every 5th milliseconds.
+
+
+
+DigitalOut LED_GREEN(PA_4);
+DigitalOut LED_RED(PA_5);
+DigitalOut LED_ENABLE(PB_5);
+
+//optical sensors
+DigitalIn SCANNER_INPUT(PC_10);
+DigitalIn SCANNER_OUTPUT(PC_1);
+DigitalIn SWITCH_INPUT(PC_2);
+DigitalIn REJECT_INPUT(PA_7);
+DigitalIn REJECT_OUTPUT(PA_6);
+DigitalIn STORAGE_INPUT(PC_3);
+
+int opticalSensors[6] = {0};    //list to store the values from optical sensors
+int lastValueOpticalSensors[6] = {0};    //list to store the previous values from optical sensors
+float sensorNoteRunningTimes[10] = {0};  //The times (in us) when note hit the sensors are stored in this array
+int pulseCountBetweenSensors[10] = {0};  //The counted number encoder pulses between sensors
+int noteInSystem = 0;
+long lastPulseTime = 0;
+long maxTimeBetweenPulses = 0;
+long minTimeBetweenPulses = 10000000;
+long timeBetweenPulses = 0;
+double currentSpeed = 0; //speed in mm/ms
+long lastControl = 0;
+
+PID controller(4.0, 0.0, 0.0, RATE);
+
+
+int AtoB_dist = 357;    //distance from SCANNER_INPUT to SCANNER_OUTPUT     counted pulses: 372
+int BtoC_dist = 81;     //distance from SCANNER_OUTPUT to SWITCH_INPUT      counted pulses: 82
+int CtoD_dist = 201;    //distance from SWITCH_INPUT to REJECT_INPUT        counted pulses: 203
+int DtoE_dist = 158;    //distance from REJECT_INPUT to REJECT_OUTPUT       counted pulses: 156
+int Tot_dist = 797;     //distance from SCANNER_INPUT to REJECT_OUTPUT      counted pulses: 813
+
+int TenEURnoteLength = 127;
+int FiftyEURnoteLength = 140;
+int TwentyEURnoteLength = 120;
+
+//encoders
+InterruptIn MAIN_ENC(PA_8);
+//InterruptIn STORAGE_ENC(PA_15);
+//InterruptIn SWITCH_ENC(PB_4);
+//InterruptIn ROUTER_ENC(PB_6);
+
+
+//Motors
+PwmOut MAIN_PWM(PC_7);   //PWM to Main Drive
+DigitalOut MAIN_DIR(PC_0);   //Direction of main drive
+
+PwmOut SWITCH_PWM(PB_14);   //PWM to Switch
+DigitalOut SWITCH_DIR(PB_15);   //Direction of Switch
+
+PwmOut ROUTER_PWM(PC_8);   //PWM to Router
+DigitalOut ROUTER_DIR(PB_7);   //Direction of Router
+
+PwmOut STORAGE_PWM(PC_6);   //PWM to Storage
+DigitalOut STORAGE_DIR(PC_9);   //Direction of Storage
+
+
+
+uint32_t blinkTime_ms = 0;
+uint32_t reportTimer = 0;
+uint32_t pulses = 0;
+
+
+Timer t;
+//PID controller(1, 0, 0, RATE);    //Kc, Ti, Td, interval
+
+
+Serial pc(USBTX, USBRX);
+SWO_Channel swo("channel");
+
+void green_blink(void)
+{
+    if(t.read_ms() - blinkTime_ms > 200) {
+        LED_RED = 0;
+        LED_GREEN = !LED_GREEN;
+        blinkTime_ms = t.read_ms();
+    }
+}
+void red_blink(void)
+{
+    if(t.read_ms() - blinkTime_ms > 200) {
+        LED_GREEN = 0;
+        LED_RED = !LED_RED;
+        blinkTime_ms = t.read_ms();
+    }
+}
+
+void reportNoteThroughSensorLengths()
+{
+
+    swo.printf("\n");
+
+    //Time for note to travel through first sensor
+    long time = (sensorNoteRunningTimes[1] - sensorNoteRunningTimes[0]); //us
+    swo.printf("Time for note to pass 1th sensor (us): %d\r", time);
+    //int length = TwentyEURnoteLength;
+    int countedPulses = pulseCountBetweenSensors[1] - pulseCountBetweenSensors[0];
+    swo.printf("  Counted pulses: %d\r\n", countedPulses);
+
+    //Time for note to travel through second sensor
+    time = (sensorNoteRunningTimes[3] - sensorNoteRunningTimes[2]); //us
+    swo.printf("Time for note to pass 2th sensor (us): %d\r\n", time);
+    countedPulses = pulseCountBetweenSensors[3] - pulseCountBetweenSensors[2];
+    swo.printf("  Counted pulses: %d\r\n", countedPulses);
+
+    //Time for note to travel through third sensor
+    time = (sensorNoteRunningTimes[5] - sensorNoteRunningTimes[4]); //us
+    swo.printf("Time for note to pass 3th sensor (us): %d\r\n", time);
+    countedPulses = pulseCountBetweenSensors[5] - pulseCountBetweenSensors[4];
+    swo.printf("  Counted pulses: %d\r\n", countedPulses);
+
+    //Time for note to travel through fourth sensor
+    time = (sensorNoteRunningTimes[7] - sensorNoteRunningTimes[6]); //us
+    swo.printf("Time for note to pass 4th sensor (us): %d\r\n", time);
+    countedPulses = pulseCountBetweenSensors[7] - pulseCountBetweenSensors[6];
+    swo.printf("  Counted pulses: %d\r\n", countedPulses);
+
+    //Time for note to travel through fifth sensor
+    time = (sensorNoteRunningTimes[9] - sensorNoteRunningTimes[8]); //us
+    swo.printf("Time for note to pass 5th sensor (us): %d\r\n", time);
+    countedPulses = pulseCountBetweenSensors[9] - pulseCountBetweenSensors[8];
+    swo.printf("  Counted pulses: %d\r\n", countedPulses);
+
+    swo.printf("\n");
+    swo.printf("\n");
+
+}
+
+void report(void)
+{
+    //uint32_t now = t.read_ms();
+
+    /*
+    swo.printf("Optical sensor status: ");
+    for(int j = 0; j <=5 ; j++) {
+        swo.printf("%d ", opticalSensors[j]);
+    }
+    swo.printf("\n");
+    */
+
+    long time = (sensorNoteRunningTimes[2] - sensorNoteRunningTimes[0]) / 1000; //ms
+
+    //swo.printf("Time between first two sensors (ms): ");
+    //swo.printf("%d ", time);
+    //swo.printf("\n");
+
+    //V = S / T
+    float V = 1000 * AtoB_dist / time; //(mm/s)
+
+    swo.printf("Speed between first two sensors (mm/s): ");
+    swo.printf("%f ", V);
+    swo.printf("  Counted encoder pulses: ");
+    uint32_t countedPulses = pulseCountBetweenSensors[2] - pulseCountBetweenSensors[0];
+    swo.printf("%d ", countedPulses);
+    swo.printf("  ->Encoder speed (mm/s): ");
+    float countedSpeed = 1000 * countedPulses / time;
+    swo.printf("%f ", countedSpeed);
+    swo.printf("  ->Speed difference (percentage): ");
+    double difference = (100 * V / countedSpeed) - 100;
+    swo.printf("%f ", difference);
+
+    swo.printf("\n");
+
+    time = (sensorNoteRunningTimes[4] - sensorNoteRunningTimes[2]) / 1000; //ms
+    V = 1000 * BtoC_dist / time; //(mm/s)
+
+    swo.printf("Speed between second and third sensor (mm/s): ");
+    swo.printf("%f ", V);
+    swo.printf("  Counted encoder pulses: ");
+    countedPulses = pulseCountBetweenSensors[4] - pulseCountBetweenSensors[2];
+    swo.printf("%d ", countedPulses);
+    swo.printf("  ->Encoder speed (mm/s): ");
+    countedSpeed = 1000 * countedPulses / time;
+    swo.printf("%f ", countedSpeed);
+    swo.printf("  ->Speed difference (percentage): ");
+    difference = (100 * V / countedSpeed) - 100;
+    swo.printf("%f ", difference);
+
+    swo.printf("\n");
+
+    time = (sensorNoteRunningTimes[6] - sensorNoteRunningTimes[4]) / 1000; //ms
+    V = 1000 * CtoD_dist / time; //(mm/s)
+
+    swo.printf("Speed between third and fourth sensor (mm/s): ");
+    swo.printf("%f ", V);
+    swo.printf("  Counted encoder pulses: ");
+    countedPulses = pulseCountBetweenSensors[6] - pulseCountBetweenSensors[4];
+    swo.printf("%d ", countedPulses);
+    swo.printf("  ->Encoder speed (mm/s): ");
+    countedSpeed = 1000 * countedPulses / time;
+    swo.printf("%f ", countedSpeed);
+    swo.printf("  ->Speed difference (percentage): ");
+    difference = (100 * V / countedSpeed) - 100;
+    swo.printf("%f ", difference);
+
+    swo.printf("\n");
+
+    time = (sensorNoteRunningTimes[8] - sensorNoteRunningTimes[6]) / 1000; //ms
+    V = 1000 * DtoE_dist / time; //(mm/s)
+
+    swo.printf("Speed between fourth and fifth sensor (mm/s): ");
+    swo.printf("%f ", V);
+    swo.printf("  Counted encoder pulses: ");
+    countedPulses = pulseCountBetweenSensors[8] - pulseCountBetweenSensors[6];
+    swo.printf("%d ", countedPulses);
+    swo.printf("  ->Encoder speed (mm/s): ");
+    countedSpeed = 1000 * countedPulses / time;
+    swo.printf("%f ", countedSpeed);
+    swo.printf("  ->Speed difference (percentage): ");
+    difference = (100 * V / countedSpeed) - 100;
+    swo.printf("%f ", difference);
+
+    swo.printf("\n");
+    swo.printf("\n");
+
+    time = (sensorNoteRunningTimes[8] - sensorNoteRunningTimes[0]) / 1000; //ms
+    V = 1000 * Tot_dist / time; //(mm/s)
+
+    swo.printf("Total mean speed between first and last sensor (mm/s): ");
+    swo.printf("%f ", V);
+    swo.printf("  Total counted encoder pulses: ");
+    countedPulses = pulseCountBetweenSensors[8] - pulseCountBetweenSensors[0];
+    swo.printf("%d ", countedPulses);
+    swo.printf("  ->Encoder speed (mm/s): ");
+    countedSpeed = 1000 * countedPulses / time;
+    swo.printf("%f ", countedSpeed);
+    swo.printf("  ->Total speed difference (percentage): ");
+    difference = (100 * V / countedSpeed) - 100;
+    swo.printf("%f ", difference);
+
+    swo.printf("\n");
+    swo.printf("\n");
+
+    swo.printf("Maximum time between pulses (us): %d\r\n", maxTimeBetweenPulses);
+    swo.printf("Minimum time between pulses (us): %d\r\n", minTimeBetweenPulses);
+
+    swo.printf("\n");
+    swo.printf("Filtered speed value to PID (mm/s): ");
+    swo.printf("%f ", currentSpeed);
+    
+
+    swo.printf("\n");
+    swo.printf("\n");
+
+
+    reportNoteThroughSensorLengths();
+    
+    maxTimeBetweenPulses = 0;
+    minTimeBetweenPulses = 10000000;
+    pulses = 0;
+
+    reportTimer = t.read_ms();
+}
+
+
+
+void encTick(void)
+{
+    long now = t.read_us();
+    timeBetweenPulses = now - lastPulseTime;
+
+    if(noteInSystem) {
+        if(timeBetweenPulses > maxTimeBetweenPulses) maxTimeBetweenPulses = timeBetweenPulses;  //storing maximum time between pulses
+        if(timeBetweenPulses < minTimeBetweenPulses) minTimeBetweenPulses = timeBetweenPulses;  //storing minimum time between pulses
+    }
+
+    lastPulseTime = now;
+
+    pulses++;
+
+    //filtered 10%
+    currentSpeed = 0.05 * (1000000/timeBetweenPulses) + 0.95 * currentSpeed;  //mm/s
+    //currentSpeed = 1000000/timeBetweenPulses;  //mm/s
+}
+
+void checkOpticalSensors(void)
+{
+
+    for(int i=0; i<=5; i++) {
+        opticalSensors[i]=0;
+    }
+
+    if(SCANNER_INPUT) {
+        opticalSensors[0]=1;
+        if(lastValueOpticalSensors[0] == 0) {
+            sensorNoteRunningTimes[0] = t.read_us();    //leading edge of note detected on first optical sensor - storing time
+            noteInSystem = 1;   //note entered the system
+            pulseCountBetweenSensors[0] = pulses;
+        }
+    } else {
+        if(lastValueOpticalSensors[0] == 1) {
+            sensorNoteRunningTimes[1] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
+            pulseCountBetweenSensors[1] = pulses;
+        }
+    }
+
+    if(SCANNER_OUTPUT) {
+        opticalSensors[1]=1;
+        if(lastValueOpticalSensors[1] == 0) {
+            sensorNoteRunningTimes[2] = t.read_us();    //note detected on second optical sensor - storing time
+            pulseCountBetweenSensors[2] = pulses;
+        }
+    } else {
+        if(lastValueOpticalSensors[1] == 1) {
+            sensorNoteRunningTimes[3] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
+            pulseCountBetweenSensors[3] = pulses;
+        }
+    }
+
+    if(SWITCH_INPUT) {
+        opticalSensors[2]=1;
+        if(lastValueOpticalSensors[2] == 0) {
+            sensorNoteRunningTimes[4] = t.read_us();    //note detected on second optical sensor - storing time
+            pulseCountBetweenSensors[4] = pulses;
+        }
+    } else {
+        if(lastValueOpticalSensors[2] == 1) {
+            sensorNoteRunningTimes[5] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
+            pulseCountBetweenSensors[5] = pulses;
+        }
+    }
+
+    if(REJECT_INPUT) {
+        opticalSensors[3]=1;
+        if(lastValueOpticalSensors[3] == 0) {
+            sensorNoteRunningTimes[6] = t.read_us();    //note detected on second optical sensor - storing time
+            pulseCountBetweenSensors[6] = pulses;
+        }
+    } else {
+        if(lastValueOpticalSensors[3] == 1) {
+            sensorNoteRunningTimes[7] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
+            pulseCountBetweenSensors[7] = pulses;
+        }
+    }
+
+    if(REJECT_OUTPUT) {
+        opticalSensors[4]=1;
+        if(lastValueOpticalSensors[4] == 0) {
+            sensorNoteRunningTimes[8] = t.read_us();    //note detected on second optical sensor - storing time
+            pulseCountBetweenSensors[8] = pulses;
+        }
+    } else {
+        if(lastValueOpticalSensors[4] == 1) {
+            sensorNoteRunningTimes[9] = t.read_us();    //leaving edge of note detected on first optical sensor - storing time
+            pulseCountBetweenSensors[9] = pulses;
+            noteInSystem = 0;   //note left the system
+            report();
+        }
+    }
+
+    if(STORAGE_INPUT) opticalSensors[5]=1;
+
+    //storing sensor state in previous value array
+    for(int i=0; i<=5; i++) {
+        lastValueOpticalSensors[i] = opticalSensors[i];
+    }
+
+}
+
+void tunePID()
+{
+
+    swo.printf("setting 60percent output %\n");
+    MAIN_PWM.write(0.6f);
+    swo.printf("waiting for speedup... %\n");
+    wait(2);    //waiting 2 sec
+    swo.printf("Current speed (mm/s): %f\n", currentSpeed);
+    double beforeSpeed = currentSpeed;
+    swo.printf("setting 70percent output %\n");
+    MAIN_PWM.write(0.7f);   //10% more output
+    swo.printf("waiting for speedup... %\n");
+    wait(2);    //waiting 2 sec
+    swo.printf("Current speed (mm/s): %f\n", currentSpeed);
+    double deltaSpeed = currentSpeed - beforeSpeed;
+    swo.printf("delta Speed (mm/s): %f\n", deltaSpeed);
+    double K = deltaSpeed / 10;
+    swo.printf("PID K-factor (deltaSpeed/10): %f\n", K);
+
+    controller.setTunings(K/2, 0, 0); //K/2?
+
+    controller.setInputLimits(0.0, 600);
+
+    //Pwm output from 0.0 to 1.0
+    controller.setOutputLimits(0.0, 1.0);
+
+    controller.setMode(AUTO_MODE);
+
+    controller.setSetPoint(600);
+
+    swo.printf("\n");
+    swo.printf("\n");
+    swo.printf("\n");
+
+}
+
+
+int main()
+{
+    LED_ENABLE = 1; //turning leds on
+    LED_GREEN = 0;
+    LED_RED = 0;
+
+    //Setting motors off
+    MAIN_PWM.write(0.00f);
+    SWITCH_PWM.write(0.00f);
+    ROUTER_PWM.write(0.00f);
+    STORAGE_PWM.write(0.00f);
+    MAIN_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
+    SWITCH_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
+    ROUTER_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
+    STORAGE_PWM.period(0.00005);  //Set motor PWM periods to 20KHz.
+
+
+    swo.printf("STARTED %\n");
+    swo.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);    //SYTEM CLOCK 72MHz
+    MAIN_ENC.rise(&encTick);  // attach the address of the encTick function to the rising edge
+
+    t.start();
+
+    //MAIN_PWM.write(0.7f); //40% works 0.4f
+
+    tunePID();
+
+
+    while(1) {
+
+
+        checkOpticalSensors();  //updating opticalSensors array
+
+        //if(t.read_ms() - reportTimer > 1000 && noteInSystem == 0) report();  //reporting every second
+
+        if(t.read_ms() -  lastControl > RATE) {
+            //Update the process variable.
+            controller.setProcessValue(currentSpeed);
+            //Set the new output.
+            double output = controller.compute();
+            MAIN_PWM.write(output);
+            //swo.printf("sent to controller: %f\n", output);
+            lastControl = t.read_ms();
+        }
+
+    }
+
+
+}
+
+