File content as of revision 0:fc81857d8067:

#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();
        }

    }


}