Diff: Controller.cpp

Revision:

0:380207fcb5c1

Child:

1:d5c5bb30ac90

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Thu Mar 29 07:02:09 2018 +0000
@@ -0,0 +1,263 @@
+/*
+ * 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::getGammaX() {
+    
+    return gammaX;
+}
+
+
+float Controller::getGammaY() {
+    
+    return gammaY;
+}
+
+
+float Controller::getGammaZ() {
+    
+    return gammaZ;
+}
+
+
+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;
+      
+    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
+        
+        // Calculate duty cycles from desired Torque
+        
+        // actual robot pose
+        
+        // set new gamma's, phi's
+        previousGammaX = gammaX;
+        previousGammaY = gammaY;
+        previousGammaZ = gammaZ;
+        previousPhiX = phiX;
+        previousPhiY = phiY;
+    }
+};