File content as of revision 0:fbdae7e6d805:

/*
 * 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::COS_ALPHA = 0.707106781186548;           // cos(alpha)
const float Controller::SIN_ALPHA = 0.707106781186548;           // cos(alpha)
const float Controller::RB = 0.09f;                            // Ball Radius in [m]
const float Controller::RW = 0.029f;                            // 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 = 3.5f;
const float Controller::MB = 0.6;                                // masse der Ball
/**
 * 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(20.0f);

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

    M3Filter.setPeriod(PERIOD);
    M3Filter.setFrequency(20.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);
    
    
    
    gainG = 1.0f;
    gain_dG = 1.0f;
    offsetX = 1.0f;
    offsetY = 1.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);
//}

float Controller::sign(float v)
{
    if (v < 0.5) return -1;
    if (v > 0.5) return 1;
    return 0;

}

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[4] = {0.0036,4.1925,0.0231,1.2184};


    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 * Mes1 = new float[2000];
    float * Mes2 = new float[2000];
    float * Mes3 = new float[2000];
    float * Mes4 = new float[2000];
    float * Mes5 = new float[2000];
    float * Mes6 = new float[2000];
    */

    int a = 0;

    // escon I/O
    AnalogIn M1_AOUT1(PC_4); // neu
    AnalogIn M1_AOUT2(PA_5); // neu

    AnalogIn M2_AOUT1(PC_1); // neu
    AnalogIn M2_AOUT2(PC_0); // neu

    AnalogIn M3_AOUT1(PA_7); // neu
    AnalogIn M3_AOUT2(PA_4); // neu


    while (true) {

        // wait for the periodic signal

        thread.signal_wait(signal);


        //pc1.printf("controller\r\n");


        /*
        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\r\n",*(T+j),*(Mes1+j),*(Mes2+j),*(Mes3+j),*(Mes4+j),*(Mes5+j),*(Mes6+j));
            }
            pc1.printf("fine dati:\r\n\n");
            delete T;
            delete Mes1;
            delete Mes2;
            delete Mes3;
            delete Mes4;
            delete Mes5;
            delete Mes6;
        }
        */

        // gain correction vie bluetooth
        //if(com.dataRecived){gainG = com.gainG; gain_dG = com.gain_dG; com.dataRecived = false;}

        //pc1.printf("data recived %d gainG = %.7f gain_d = %.7f\r\n",com.dataRecived,com.gainG,com.gain_dG);
        //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();

        gammaZ = 0.0f;//0.0175f*cos(18.0f*t);


        // 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);

        //printf("%.7f %.7f\r\n", gammaX-0.0025f,gammaY-0.043f);

        //pc1.printf("gainG = %.7f gain_d = %.7f\r\n",gainG,gain_dG);

        float x[4] = {0.0f,gammaY,0.0f,d_gammaY};

        /*
        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;
        }
        */
        
        float k0 = 0.037f;
        float k1 = 13.5f*gainG;
        float k2 = 0.02f;
        float k3 = 1.185868f*gain_dG;
        
        float Mx = k1*(gammaX -0.0238f*offsetX) + k3*d_gammaX;
        float My = k1*(gammaY -0.0167f*offsetY) + k3*d_gammaY;
        float Mz = 0.0f;
        
        M1 = (2.0f/(3.0f*COS_ALPHA))*Mx + (1.0f/(3.0f*SIN_ALPHA))*Mz;
        M2 = (-1.0f/(3.0f*COS_ALPHA))*Mx + (SQRT_3/(3.0f*COS_ALPHA))*My + (1.0f/(3.0f*SIN_ALPHA))*Mz;
        M3 = (-1.0f/(3.0f*COS_ALPHA))*Mx + (-SQRT_3/(3.0f*COS_ALPHA))*My + (1.0f/(3.0f*SIN_ALPHA))*Mz;

        M1motion.incrementToVelocity(M1, PERIOD);
        M2motion.incrementToVelocity(M2, PERIOD);
        M3motion.incrementToVelocity(M3, PERIOD);


        //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 = -M1motion.velocity*1000.0f/KI;
        float I2 = -M2motion.velocity*1000.0f/KI;
        float I3 = -M3motion.velocity*1000.0f/KI;

        /*
        if(t<20001) {
            if (t % 10 == 0) {
                *(T+a) = t;
                *(Mes1+a) = M1; //M1_AOUT1.read()*3.3f*4000.0f/3.0f - 2000.0f;
                *(Mes2+a) = M1motion.velocity;
                *(Mes3+a) = -I1*KI/1000.0f;
                *(Mes4+a) = M2;
                *(Mes5+a) = M2motion.velocity; //[rad/s]
                *(Mes6+a) = -I2*KI/1000.0f;
                a++;
            }
        }
        */
        
        // security constrain that retard motor start
        if(t<1000.0f){I1 = 0.0f; I2 = 0.0f; I3 = 0.0f;}

        float dutyCycle1 = 0.4f/6.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/6.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/6.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);



        //pc1.printf("%.7f %.7f %.7f\r\n", dutyCycle1,dutyCycle2,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");

    }
};