File content as of revision 0:02dd72d1d465:

/*
 * Controller.cpp
 * Copyright (c) 2018, ZHAW
 * All rights reserved.
 *
 *  Created on: 27.03.2018
 *      Author: BaBoRo Development Team
 */

#include <cmath>
#include "Controller.h"

using namespace std;

const float Controller::PERIOD = 0.001f;                        // the period of the timer interrupt, given in [s]
const float Controller::ALPHA = 0.785398163397448f;             // alpha = 45° in [rad]
const float Controller::RB = 0.0925f;                            // Ball Radius in [m]
const float Controller::RW = 0.024f;                            // Wheel Radius in [m]
const float Controller::PI = 3.141592653589793f;                // PI
const float Controller::SQRT_3 = 1.732050807568877f;            // Square root of 3
const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;      // frequency of lowpass filter for actual speed values, given in [rad/s]
const float Controller::COUNTS_PER_TURN = 4096.0f;
const float Controller::KI = 58.5f;                             // torque constant [mNm/A]
const float Controller::MIN_DUTY_CYCLE = 0.1f;                  // minimum allowed value for duty cycle (10%)
const float Controller::MAX_DUTY_CYCLE = 0.9f;                  // maximum allowed value for duty cycle (90%)
const float Controller::MAX_ACC_M = 2.0f;
/**
 * Create and initialize a robot controller object.
 * @param pwm0 a pwm output object to set the duty cycle for the first motor.
 * @param pwm1 a pwm output object to set the duty cycle for the second motor.
 * @param pwm2 a pwm output object to set the duty cycle for the third motor.
 * @param counter1
 */
Controller::Controller(PwmOut& pwm0, PwmOut& pwm1, PwmOut& pwm2, EncoderCounter& counter1, EncoderCounter& counter2, EncoderCounter& counter3, IMU& imu) : pwm0(pwm0), pwm1(pwm1), pwm2(pwm2), counter1(counter1), counter2(counter2), counter3(counter3), imu(imu), thread(osPriorityHigh, STACK_SIZE)
{

    // initialize local values

    pwm0.period(0.0005f);// 0.5ms 2KHz
    pwm0.write(0.5f);

    pwm1.period(0.0005f);
    pwm1.write(0.5f);

    pwm2.period(0.0005f);
    pwm2.write(0.5f);

    gammaX = imu.getGammaX();
    gammaY = imu.getGammaY();
    gammaZ = imu.getGammaZ();

    phiX = 0.0f;
    phiY = 0.0f;

    d_phiX = 0.0f;
    d_phiY = 0.0f;

    x = 0.0f;
    y = 0.0f;

    float previousValueCounter1 = counter1.read();
    float previousValueCounter2 = counter2.read();
    float previousValueCounter3 = -counter3.read();

    speedFilter1.setPeriod(PERIOD);
    speedFilter1.setFrequency(LOWPASS_FILTER_FREQUENCY);

    speedFilter2.setPeriod(PERIOD);
    speedFilter2.setFrequency(LOWPASS_FILTER_FREQUENCY);

    speedFilter3.setPeriod(PERIOD);
    speedFilter3.setFrequency(LOWPASS_FILTER_FREQUENCY);

    d_phiXFilter.setPeriod(PERIOD);
    d_phiXFilter.setFrequency(10.0f);

    d_phiYFilter.setPeriod(PERIOD);
    d_phiYFilter.setFrequency(10.0f);

    gammaXFilter.setPeriod(PERIOD);
    gammaXFilter.setFrequency(100.0f);
    gammaYFilter.setPeriod(PERIOD);
    gammaYFilter.setFrequency(100.0f);
    d_gammaXFilter.setPeriod(PERIOD);
    d_gammaXFilter.setFrequency(100.0f);
    d_gammaYFilter.setPeriod(PERIOD);
    d_gammaYFilter.setFrequency(100.0f);

    M1Filter.setPeriod(PERIOD);
    M1Filter.setFrequency(50.0f);

    M2Filter.setPeriod(PERIOD);
    M2Filter.setFrequency(50.0f);

    M3Filter.setPeriod(PERIOD);
    M3Filter.setFrequency(50.0f);

    previousValueCounter1 = 0.0f;
    previousValueCounter2 = 0.0f;
    previousValueCounter3 = 0.0f;

    actualSpeed1 = 0.0f;
    actualSpeed2 = 0.0f;
    actualSpeed3 = 0.0f;

    gammaXref = 0.0f;
    gammaYref = 0.0f;
    gammaZref = 0.0f;
    phiXref = 0.0f;
    phiYref = 0.0f;

    M1motion.setProfileVelocity(0.4f);
    M1motion.setProfileAcceleration(MAX_ACC_M);
    M1motion.setProfileDeceleration(MAX_ACC_M);

    M2motion.setProfileVelocity(0.4f);
    M2motion.setProfileAcceleration(MAX_ACC_M);
    M2motion.setProfileDeceleration(MAX_ACC_M);

    M3motion.setProfileVelocity(0.4f);
    M3motion.setProfileAcceleration(MAX_ACC_M);
    M3motion.setProfileDeceleration(MAX_ACC_M);

    d_phiXMotion.setProfileVelocity(0.5f);
    d_phiXMotion.setProfileAcceleration(1.0f);
    d_phiXMotion.setProfileDeceleration(2.0f);

    d_phiYMotion.setProfileVelocity(0.5f);
    d_phiYMotion.setProfileAcceleration(1.0f);
    d_phiYMotion.setProfileDeceleration(2.0f);

    // start thread and timer interrupt

    thread.start(callback(this, &Controller::run));
    //ticker.attach(callback(this, &Controller::sendSignal), PERIOD);
}

/**
 * Delete the robot controller object and release all allocated resources.
 */
Controller::~Controller()
{

    //ticker.detach();
}

// set --------------------------------
void Controller::setGammaX(float gammaX)
{

    this->gammaX = gammaX;
}


void Controller::setGammaY(float gammaY)
{

    this->gammaY = gammaY;
}


void Controller::setGammaZ(float gammaZ)
{

    this->gammaZ = gammaZ;
}


void Controller::setPhiX(float phiX)
{

    this->phiX = phiX;
}

void Controller::setPhiY(float phiY)
{

    this->phiY = phiY;
}

void Controller::setX(float x)
{

    this->x = x;
}

/**
 * Sets the actual y coordinate of the robots position.
 * @param y the y coordinate of the position, given in [m].
 */
void Controller::setY(float y)
{

    this->y = y;
}

// get --------------------------------

float Controller::getPhiX()
{

    return phiX;
}

float Controller::getPhiY()
{

    return phiY;
}

/**
 * Gets the actual x coordinate of the robots position.
 * @return the x coordinate of the position, given in [m].
 */
float Controller::getX()
{

    return x;
}

/**
 * Gets the actual y coordinate of the robots position.
 * @return the y coordinate of the position, given in [m].
 */
float Controller::getY()
{

    return y;
}

/**
 * This method is called by the ticker timer interrupt service routine.
 * It sends a signal to the thread to make it run again.
 */
//void Controller::sendSignal() {

//    thread.signal_set(signal);
//}

/**
 * This <code>run()</code> method contains an infinite loop with the run logic.
 */
void Controller::run()
{
    Serial pc1(USBTX, USBRX); // tx, rx
    pc1.baud(100000);

    float integratedGammaX = 0.0f;
    float integratedGammaY = 0.0f;
    float integratedGammaZ = 0.0f;
    float integratedPhiX = 0.0f;
    float integratedPhiY = 0.0f;

    float previousGammaX = 0.0;
    float previousGammaY = 0.0;
    float previousGammaZ = 0.0;
    float previousPhiX = 0.0;
    float previousPhiY = 0.0;

    // K matrix
    /*
    float K[3][10] = {
        {0.002327,0.000000,2.166704,0.000000,0.055133,0.032581,0.000000,0.517927,0.000000,0.031336},
        {-0.001164,0.002015,-1.083352,1.876421,0.055133,-0.016291,0.028216,-0.258963,0.448538,0.031336},
        {-0.001164,-0.002015,-1.083352,-1.876421,0.055133,-0.016291,-0.028216,-0.258963,-0.448538,0.031336}
    };
    */

    float K[3][10] = {
        {0.002327,0.000000,2.362528,-0.000000,0.055133,0.009269,0.000000,0.499429,0.000000,0.031336},
        {-0.001164,0.002015,-1.181264,2.046009,0.055133,-0.004635,0.008027,-0.249714,0.432518,0.031336},
        {-0.001164,-0.002015,-1.181264,-2.046009,0.055133,-0.004635,-0.008027,-0.249714,-0.432518,0.031336}
    };


    float rad1 = 0.0f;
    float rad2 = 0.0f;
    float rad3 = 0.0f;

    float rad1contr = 0.0f;

    long int t = 0;

    // messung
    int * T = new int[2000];
    //float * PX = new float[5000];
    //float * PY = new float[5000];
    float * GX = new float[2000];
    float * GY = new float[2000];
    float * GZ = new float[2000];
    //float * dPX = new float[5000];
    //float * dPY = new float[5000];
    float * dGX = new float[2000];
    float * dGY = new float[2000];
    float * dGZ = new float[2000];
    float * W1 = new float[2000];
    float * W2 = new float[2000];
    float * W3 = new float[2000];

    int a = 0;

    AnalogIn M3_AOUT1(PA_7);
    
    pwm0.write(0.6);
    pwm1.write(0.6);
    pwm2.write(0.6);

    while (true) {
        
        /*
        if(t==21000) {
            pc1.printf("invio dati:\r\n\n");
            for(int j=0; j<2000; j++) {
                //pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(PX+j),*(PY+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(dPX+j),*(dPY+j),*(W1+j),*(W2+j),*(W3+j));
                pc1.printf("%d %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",*(T+j),*(GX+j),*(GY+j),*(GZ+j),*(dGX+j),*(dGY+j),*(dGZ+j),*(W1+j),*(W2+j),*(W3+j));
            }
            pc1.printf("fine dati:\r\n\n");
            delete T;
            //delete PX;
            //delete PY;
            delete GX;
            delete GY;
            delete GZ;
            //delete dPX;
            //delete dPY;
            delete dGX;
            delete dGY;
            delete dGZ;
            delete W1;
            delete W2;
            delete W3;

        }
        */




        mutex.lock();
        //printf("Controller start\r\n");

        gammaX = imu.getGammaX();
        gammaY = imu.getGammaY();
        gammaZ = imu.getGammaZ();
        d_gammaX = imu.getDGammaX();
        d_gammaY = imu.getDGammaY();
        d_gammaZ = imu.getDGammaZ();
        
        

        // wait for the periodic signal

        // thread.signal_wait(signal);

        //printf("%d %d %d\r\n",counter1.read(),counter2.read(),counter3.read());
        //pwm0.write(0.6f);
        //pwm1.write(0.6f);
        //pwm2.write(0.6f);




        // Calculate Ball Velocities
        short valueCounter1 = counter1.read();
        short valueCounter2 = counter2.read();
        short valueCounter3 = -counter3.read();

        short countsInPastPeriod1 = valueCounter1-previousValueCounter1;
        short countsInPastPeriod2 = valueCounter2-previousValueCounter2;
        short countsInPastPeriod3 = valueCounter3-previousValueCounter3;

        previousValueCounter1 = valueCounter1;
        previousValueCounter2 = valueCounter2;
        previousValueCounter3 = valueCounter3;

        actualSpeed1 = speedFilter1.filter((2*PI)*(float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
        actualSpeed2 = speedFilter2.filter((2*PI)*(float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]
        actualSpeed3 = speedFilter3.filter((2*PI)*(float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD); // actual speed motor1 in [rad/s]

        rad1 += actualSpeed1*PERIOD;
        rad2 += actualSpeed2*PERIOD;
        rad3 += actualSpeed3*PERIOD;

        //printf("rad1 = %.7f rad2 = %.7f rad3 = %.7f\r\n",rad1,rad2,rad3);

        //printf("%.7f %.7f %.7f %.7f %.7f %.7f\r\n",(2*PI)*(float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD,actualSpeed1,(2*PI)*(float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD,actualSpeed2,(2*PI)*(float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD,actualSpeed3);


        //printf("%.7f %.7f %.7f\r\n",actualSpeed1/2*PI,actualSpeed2/2*PI,actualSpeed3/2*PI);

        float actualDPhiX = d_phiXFilter.filter(RW/RB*((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*0.0f)/(2*cos(ALPHA))+d_gammaX);
        float actualDPhiY = d_phiYFilter.filter((RW/RB*(actualSpeed2 - actualSpeed3))/(SQRT_3*cos(ALPHA))+d_gammaY); //float actualDPhiY = d_phiYFilter.filter((RW/RB*(actualSpeed2 - actualSpeed3 - actualSpeed1)+cos(ALPHA)*(actualDPhiX-gammaX)+sin(ALPHA)*0.0f)/(SQRT_3*cos(ALPHA))+d_gammaY);

        //printf("%.7f %.7f %.7f\r\n",d_gammaX,d_gammaY,d_gammaZ);

        //this->d_phiX = RB/RW*(((actualSpeed1 - actualSpeed2 - actualSpeed3) - sin(ALPHA)*RB/RW*d_gammaZ)/(2*cos(ALPHA))+d_gammaX);
        //this->d_phiY = RB/RW*((actualSpeed2 - actualSpeed3)/(SQRT_3*cos(ALPHA))+d_gammaY);

        this->d_phiX = actualDPhiX;
        this->d_phiY = actualDPhiY;

        this->phiX = phiX + actualDPhiX*PERIOD;
        this->phiY = phiY + actualDPhiY*PERIOD;




        //printf("phiX = %.5f phiY = %.5f\r\n",phiX/PI*180.0f,phiY/PI*180.0f);
        //printf("%.5f %.5f\r\n",phiX,phiY);
        //printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,actualSpeed1,actualSpeed2,actualSpeed3);
        //printf("%.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ);

        // calculate actual error
        float errorGammaX = -gammaXref + gammaX;
        float errorGammaY = -gammaYref + gammaY;
        float errorGammaZ = -gammaZref + gammaZ;
        float errorPhiX = -phiXref + phiX;
        float errorPhiY = -phiYref + phiY;

        // Integration states
        integratedGammaX = integratedGammaX + (errorGammaX-previousGammaX)*PERIOD;
        integratedGammaY = integratedGammaY + (errorGammaY-previousGammaY)*PERIOD;
        integratedGammaZ = integratedGammaZ + (errorGammaZ-previousGammaZ)*PERIOD;
        integratedPhiX = integratedPhiX + (errorPhiX-previousPhiX)*PERIOD;
        integratedPhiY = integratedPhiY + (errorPhiY-previousPhiY)*PERIOD;

        // set new gamma's, phi's
        previousGammaX = errorGammaX;
        previousGammaY = errorGammaY;
        previousGammaZ = errorGammaZ;
        previousPhiX = errorPhiX;
        previousPhiY = errorPhiY;

        // Calculate Torque motors

        //[M1,M2,M3]=K*x

        float M1 = 0.0f;
        float M2 = 0.0f;
        float M3 = 0.0f;

        d_phiXMotion.incrementToVelocity(d_phiX,PERIOD);
        d_phiYMotion.incrementToVelocity(d_phiY,PERIOD);

        float x[10][1] = {
            {0.0f},{0.0f},{(gammaX)},{(gammaY)},{0.0f},{0.0f},{0.0f},{d_gammaX},{d_gammaY},{0.0f}
        };

        /*
        if(gammaX<0.02f && gammaX>-0.02f) {
            x[2][0]=0.0f;
        }
        if(gammaY<0.02f && gammaY>-0.02f) {
            x[3][0]=0.0f;
        }
        if(d_gammaX<0.02f && gammaX>-0.02f) {
            x[7][0]=0.0f;
        }
        if(d_gammaY<0.02f && gammaY>-0.02f) {
            x[8][0]=0.0f;
        }
        */

        for(int i=0; i<10; i++) {
            M1 = M1 + K[0][i]*x[i][0];
            M2 = M2 + K[1][i]*x[i][0];
            M3 = M3 + K[2][i]*x[i][0];
        };
        
        printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n",gammaX,gammaY,d_gammaX,d_gammaY);
        
        M1motion.incrementToPosition(M1, PERIOD);
        M2motion.incrementToPosition(M2, PERIOD);
        M3motion.incrementToPosition(M3, PERIOD);

        if(t<20001) {
            if (t % 10 == 0) {
                *(T+a) = t;
                //*(PX+a) = phiX;
                //*(PY+a) = phiY;
                *(GX+a) = gammaXFilter.filter(gammaX);
                *(GY+a) = gammaYFilter.filter(gammaY);
                *(GZ+a) = gammaZ;
                //*(dPX+a) = d_phiX;
                //*(dPY+a) = d_phiY;
                *(dGX+a) = d_gammaXFilter.filter(d_gammaX);
                *(dGY+a) = d_gammaYFilter.filter(d_gammaY);
                *(dGZ+a) = d_gammaZ;
                *(W1+a) = M1;
                *(W2+a) = M2;
                *(W3+a) = M3;
                a++;
            }
        }

        //pc1.printf("%.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,M1,M2,M3);
        //if(t<2000) {
        //messung[0][t]=t/100.0f;
        //messung[1][t]=gammaX;
        //messung[2][t]=gammaY;
        //pc1.printf("%.2f\r\n", t/100.0f);
        //}

        //pc1.printf("%.2f %.2f %.2f\r\n", t/100.0f,gammaX,gammaY);
        //if(t>2100 && t<4000) {
        //pc1.printf("%.2f %.7f %.7f\r\n",1.0,1.0,1.0);//messung[0][t-2100], messung[1][t-2100], messung[2][t-2100]
        //}

        //printf("Mi %.7f %.7f %.7f motion Mi %.7f %.7f %.7f\r\n", M1,M2,M3,M1motion.velocity,M2motion.velocity,M3motion.velocity);
        //printf("%.7f %.7f\r\n", gammaX,gammaY);

        //printf("x:\r\n %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f %.7f\r\n M:\r\n %.7f %.7f %.7f\r\n", phiX,phiY,gammaX,gammaY,gammaZ,d_phiX,d_phiY,d_gammaX,d_gammaY,d_gammaZ,integratedPhiX,integratedPhiY,integratedGammaX,integratedGammaY,integratedGammaZ,M1,M2,M3);

        // Calculate duty cycles from desired Torque, limit and set duty cycles
        //float I1 = -M1*1000.0f/KI;
        //float I2 = -M2*1000.0f/KI;
        //float I3 = -M3*1000.0f/KI;
        float I1 = -M1motion.velocity*1000.0f/KI; //cos(0.1f*(float)t)
        float I2 = -M2motion.velocity*1000.0f/KI;
        float I3 = -M3motion.velocity*1000.0f/KI;

        //printf("%.7f %.7f\r\n",M3_AOUT1.read()* 3.3f *4.0f - 6.0f,I3);
        //printf("%.7f %.7f %.7f\r\n",I1,I2,I3);
        //printf("%.7f %.7f %.7f\r\n",phiX/PI*180.0f,phiY/PI*180.0f,phiY/PI*180.0f);

        //pc1.printf("%.7f %.7f\r\n", M1,M1motion.velocity);

        float dutyCycle1 = 0.4f/18.0f*I1 + 0.5f;
        if (dutyCycle1 < MIN_DUTY_CYCLE) {
            dutyCycle1 = MIN_DUTY_CYCLE;
        } else if (dutyCycle1 > MAX_DUTY_CYCLE) {
            dutyCycle1 = MAX_DUTY_CYCLE;
        }
        //pwm0.write(dutyCycle1);

        float dutyCycle2 = 0.4f/18.0f*I2 + 0.5f;
        if (dutyCycle2 < MIN_DUTY_CYCLE) dutyCycle2 = MIN_DUTY_CYCLE;
        else if (dutyCycle2 > MAX_DUTY_CYCLE) dutyCycle2 = MAX_DUTY_CYCLE;
        //pwm1.write(dutyCycle2);

        float dutyCycle3 = 0.4f/18.0f*I3 + 0.5f;
        if (dutyCycle3 < MIN_DUTY_CYCLE) dutyCycle3 = MIN_DUTY_CYCLE;
        else if (dutyCycle3 > MAX_DUTY_CYCLE) dutyCycle3 = MAX_DUTY_CYCLE;
        //pwm2.write(dutyCycle3);

        //printf("pwm1 = %.5f pwm2 = %.5f pwm3 = %.5f\r\n",dutyCycle1,dutyCycle2,dutyCycle3);

        // actual robot pose
        this->x = phiY*RB;
        this->y = phiX*RB;


        t++;


        //printf("Controller end\r\n");

        mutex.unlock();

        thread.wait(1.0);
    }
};