BA
Backup 1
Diff: Controller.cpp
- Revision:
- 0:02dd72d1d465
diff -r 000000000000 -r 02dd72d1d465 Controller.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp Tue Apr 24 11:45:18 2018 +0000
@@ -0,0 +1,562 @@
+/*
+ * 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);
+ }
+};