File content as of revision 2:3f836b662104:

/*
 * 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.095f;                            // 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 = 1024.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.
 */
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.002f);
    pwm0.write(0.5f);

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

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

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

    phiX = 0.0f;
    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);

    previousValueCounter1 = 0.0f;
    previousValueCounter2 = 0.0f;
    previousValueCounter3 = 0.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()
{

    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][15] = {
        {0.1010, 0.0000, 2.4661, 0.0000, 0.0639, 0.0621, 0.0000, 0.3066, 0.0000, 0.0237, 0.0816, 0.0000, 0.0000, 0.0000, 0.0577},
        {-0.0505, 0.0875, -1.2330, 2.1357, 0.0639, -0.0311, 0.0538, -0.1533, 0.2655, 0.0237, -0.0408, 0.0707, 0.0000, 0.0000, 0.0577},
        {-0.0505, -0.0875, -1.2330, -2.1357, 0.0639, -0.0311, -0.0538, -0.1533, -0.2655, 0.0237, -0.0408, -0.0707, 0.0000, 0.0000, 0.0577}
    };


    while (true) {

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

        // Read encoder data
        float w1 = counter1.read()/1024.0f *1/(2*PI); // Angle wheel 1
        float w2 = counter2.read()/1024.0f *1/(2*PI); // Angle wheel 2
        float w3 = counter3.read()/1024.0f *1/(2*PI); // Angle wheel 3

        // Calculate Ball angle in [rad] using the kinematic model
        this->phiX = ((w1 - w2 - w3) - sin(ALPHA)*RB/RW*gammaZ)/(2*cos(ALPHA))+gammaX;
        this->phiY = (w2 - w3)/(SQRT_3*cos(ALPHA))+gammaY;

        // Integration states
        integratedGammaX = integratedGammaX + (gammaX-previousGammaX)*PERIOD;
        integratedGammaY = integratedGammaY + (gammaY-previousGammaY)*PERIOD;
        integratedGammaZ = integratedGammaZ + (gammaZ-previousGammaZ)*PERIOD;
        integratedPhiX = integratedPhiX + (phiX-previousPhiX)*PERIOD;
        integratedPhiY = integratedPhiY + (phiY-previousPhiY)*PERIOD;

        // 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((float)countsInPastPeriod1/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
        actualSpeed2 = speedFilter2.filter((float)countsInPastPeriod2/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]
        actualSpeed3 = speedFilter3.filter((float)countsInPastPeriod3/COUNTS_PER_TURN/PERIOD/(2*PI)); // actual speed motor1 in [rad/s]

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

        // Calculate Torque motors

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

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

        float x[15][1] = {
            {gammaX},{gammaY},{gammaZ},{phiX},{phiY},{d_gammaX},{d_gammaY},{d_gammaZ},{d_phiX},{d_phiY},{integratedGammaX},{integratedGammaY},{integratedGammaZ},{integratedPhiX},{integratedPhiY}
        };

        for(int i=0; i<14; i++) {
            M1 = M1 + K[0][i]*x[i][1];
            M2 = M2 + K[1][i]*x[i][1];
            M3 = M3 + K[2][i]*x[i][1];
        };

        // Calculate duty cycles from desired Torque
        
        
        //pwm1.write(dutyCycle1);
        //pwm2.write(dutyCycle2);
        //pwm3.write(dutyCycle3);
        
        // actual robot pose
        this->x = phiY*RB;
        this->y = phiX*RB;
        
        // set new gamma's, phi's
        previousGammaX = gammaX;
        previousGammaY = gammaY;
        previousGammaZ = gammaZ;
        previousPhiX = phiX;
        previousPhiY = phiY;
    }
};