Diff: Controller.cpp

Revision:

1:d5c5bb30ac90

Parent:

0:380207fcb5c1

Child:

2:3f836b662104

--- a/Controller.cpp	Thu Mar 29 07:02:09 2018 +0000
+++ b/Controller.cpp	Thu Mar 29 09:25:16 2018 +0000
@@ -27,48 +27,49 @@
  * @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) {
-    
+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);
 }
@@ -76,42 +77,49 @@
 /**
  * Delete the robot controller object and release all allocated resources.
  */
-Controller::~Controller() {
-    
+Controller::~Controller()
+{
+
     //ticker.detach();
 }
 
 // set --------------------------------
-void Controller::setGammaX(float gammaX) {
-    
+void Controller::setGammaX(float gammaX)
+{
+
     this->gammaX = gammaX;
 }
 
 
-void Controller::setGammaY(float gammaY) {
-    
+void Controller::setGammaY(float gammaY)
+{
+
     this->gammaY = gammaY;
 }
 
 
-void Controller::setGammaZ(float gammaZ) {
-    
+void Controller::setGammaZ(float gammaZ)
+{
+
     this->gammaZ = gammaZ;
 }
 
 
-void Controller::setPhiX(float phiX) {
-    
+void Controller::setPhiX(float phiX)
+{
+
     this->phiX = phiX;
 }
 
-void Controller::setPhiY(float phiY) {
-    
+void Controller::setPhiY(float phiY)
+{
+
     this->phiY = phiY;
 }
 
-void Controller::setX(float x) {
-    
+void Controller::setX(float x)
+{
+
     this->x = x;
 }
 
@@ -119,38 +127,23 @@
  * 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) {
-    
+void Controller::setY(float y)
+{
+
     this->y = y;
 }
 
 // get --------------------------------
 
-float Controller::getGammaX() {
-    
-    return gammaX;
-}
-
+float Controller::getPhiX()
+{
 
-float Controller::getGammaY() {
-    
-    return gammaY;
-}
-
-
-float Controller::getGammaZ() {
-    
-    return gammaZ;
-}
-
-
-float Controller::getPhiX() {
-    
     return phiX;
 }
 
-float Controller::getPhiY() {
-    
+float Controller::getPhiY()
+{
+
     return phiY;
 }
 
@@ -158,8 +151,9 @@
  * Gets the actual x coordinate of the robots position.
  * @return the x coordinate of the position, given in [m].
  */
-float Controller::getX() {
-    
+float Controller::getX()
+{
+
     return x;
 }
 
@@ -167,8 +161,9 @@
  * Gets the actual y coordinate of the robots position.
  * @return the y coordinate of the position, given in [m].
  */
-float Controller::getY() {
-    
+float Controller::getY()
+{
+
     return y;
 }
 
@@ -177,81 +172,113 @@
  * 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() {
-    
+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;