BMT Module 9 Group 4
DemoState with IK and MotorControl and input vx_des and vy_des with potmeter1 and button2
Dependencies: FastPWM MODSERIAL Matrix MatrixMath mbed QEI
Diff: main.cpp
- Revision:
- 1:2219a519e2bf
- Parent:
- 0:550f6e86da32
- Child:
- 2:638c6155d0af
diff -r 550f6e86da32 -r 2219a519e2bf main.cpp
--- a/main.cpp Thu Nov 01 16:52:17 2018 +0000
+++ b/main.cpp Thu Nov 01 19:24:28 2018 +0000
@@ -5,15 +5,14 @@
#include "QEI.h"
#include <math.h>
MODSERIAL pc(USBTX, USBRX);
-DigitalOut motor1DirectionPin(D7);
-DigitalOut motor2DirectionPin(D4);
+DigitalOut motor1Direction(D7);
+DigitalOut motor2Direction(D4);
DigitalOut Led(LED_GREEN);
DigitalIn button2(SW3);
-FastPWM motor1MagnitudePin(D6);
-FastPWM motor2MagnitudePin(D5);
+FastPWM motor1PWM(D6);
+FastPWM motor2PWM(D5);
AnalogIn potMeter1(A4);
AnalogIn potMeter2(A5);
-InterruptIn button2(D3);
QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
@@ -22,6 +21,12 @@
Ticker printTicker;
//------------------------------------------------------------------------------
+//Global Variables
+
+double currentPosition1 = 0.0; // Starting position of motor 1 [rad]
+double currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad]
+int a = 0;
+
//Inverse Kinematics
const double L0 = 0.1; // Horizontal length from base to first joint [m] NAMETEN!!
const double L1 = 0.3; // Length link 1 [m] NAMETEN!!
@@ -30,10 +35,12 @@
volatile double q1 = 0.0; // Starting reference angle of first link [rad]
volatile double q2 = -(0.5*3.14); // Starting reference angle of second link wrt first link [rad]
double t = 2.0; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!!
+volatile double vx_des = 0.0; // Starting velocity in x-direction [rad/s]
+volatile double vy_des = 0.0; // Starting velocity in y-direction [rad/s]
Matrix Q_set(2,1); // Setting a matrix for storing the angular velocities [rad/sec]
Matrix J(2,2); // Setting a matrix for the Jacobian
Matrix V(2,1); // Setting a matrix for storing the EMG-measured velocities
-
+
// Motor Control
volatile double potMeterPosition1 = 0.0;
volatile double potMeterPosition2 = 0.0;
@@ -43,63 +50,134 @@
volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
volatile double Kd = 0.0;
volatile double Ts = 0.01; // SAMPLING TIJD!!
-
+
//------------------------------------------------------------------------------
// Potmeter values TO DETERMINE VX_DES, VY_DES
-double GetPotMeterPosition1() // Measure the current Potmeter1 value
+double GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION
{
double potMeter1In = potMeter1.read();
- potMeterPosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
- return potMeterPosition1;
+ vx_des = 0.5*3.14*potMeter1In - 0.25*3.14 ; // Reference value y, scaled to -0.25 to 0.25 revolutions
+ return vx_des;
+}
+
+double SwitchPotmeterVelocity1() { //Button has been pressed,
+ vy_des = vx_des;
+ return vy_des;
+}
+
+//------------------------------------------------------------------------------
+// KINEMATIC FUNCTIONS
+
+Matrix ComputeJ(void)
+{
+ double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
+ double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
+ double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
+ double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
+ J << a << b
+ << c << d;
+ return J;
+}
+
+Matrix ComputeV(void)
+{
+ V.add(1,1,vx_des); // Add desired x-velocity in V
+ V.add(2,1,vy_des); // Add desired y-velocity in V
+ return V;
+}
+
+double IntegrateQ1()
+{
+ q1 = q1 + (Q_set(1,1))*t; // new value for q1
+ return q1;
+}
+
+double IntegrateQ2()
+{
+ q2 = q2 + (Q_set(2,1))*t; // new value for q2
+ return q2;
+}
+
+Matrix ComputeQ_set(void)
+{
+ float a = J(1,1);
+ float b = J(1,2);
+ float c = J(2,1);
+ float d = J(2,2);
+ float e = V(1,1);
+ float f = V(2,1);
+ float first_row = a*e + b*f;
+ float second_row = c*e + d*f;
+ Q_set.add(1,1,first_row);
+ Q_set.add(2,1,second_row);
+ return Q_set;
+}
+
+double ErrorInverseKinematics1()
+{
+ double errorIK1 = q1 - currentPosition1;
+ return errorIK1;
}
-double GetPotMeterPosition2() // Measure the current Potmeter2 value
+double ErrorInverseKinematics2()
{
- double potMeter2In = potMeter2.read();
- potMeterPosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
- return potMeterPosition2;
+ double errorIK2 = q2 - currentPosition2;
+ return errorIK2;
+}
+//------------------------------------------------------------------------------
+// MOTOR PART
+//Encoder Posities
+double MeasureEncoderPosition1() // Read current position of motor 1, returns value in [rad]
+{
+ int counts1i = Encoder1.getPulses();
+ double counts1 = counts1i*1.0f;
+ double measuredPosition1 = (counts1/8400)*6.28; //Rotational position in radians
+ return measuredPosition1;
}
-// Drive the motor
-double FeedbackControl1(double Error1) // Dit moet zo geschreven worden dat het zowel met EMG als met potmeters gebruikt kan worden
+double MeasureEncoderPosition2() // Read current postion of motor 2, returns value in [rad]
+{
+ int counts2i = Encoder2.getPulses();
+ double counts2 = counts2i*1.0f;
+ double measuredPosition2 = (counts2/8400)*6.28; //Rotational position in radians
+ return measuredPosition2;
+}
+
+double FeedbackControl1(double Error1)
{
static double Error_integral1 = 0;
static double Error_prev1 = Error1;
- //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
// Proportional part:
- //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
double u_k1 = Kp * Error1;
// Integral part:
Error_integral1 = Error_integral1 + Error1 * Ts;
double u_i1 = Ki * Error_integral1;
- // Derivative part
+ // Derivative part:
double Error_derivative1 = (Error1 - Error_prev1)/Ts;
double u_d1 = Kd * Error_derivative1;
Error_prev1 = Error1;
// Sum all parts and return it
return u_k1 + u_i1 + u_d1; //motorValue
}
-
+
double FeedbackControl2(double Error2)
{
static double Error_integral2 = 0;
static double Error_prev2 = Error2;
- //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
// Proportional part:
- //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
double u_k2 = Kp * Error2;
// Integral part:
Error_integral2 = Error_integral2 + Error2 * Ts;
double u_i2 = Ki * Error_integral2;
- // Derivative part
+ // Derivative part:
double Error_derivative2 = (Error2 - Error_prev2)/Ts;
double u_d2 = Kd * Error_derivative2;
Error_prev2 = Error2;
// Sum all parts and return it
return u_k2 + u_i2 + u_d2; //motorValue
}
-
+
void SetMotor1(double motorValue1)
{
// Given -1<=motorValue<=1, this sets the PWM and direction
@@ -119,7 +197,7 @@
motor1PWM = fabs(motorValue1);
}
}
-
+
void SetMotor2(double motorValue2)
{
// Given -1<=motorValue<=1, this sets the PWM and direction
@@ -139,8 +217,8 @@
motor2PWM = fabs(motorValue2);
}
}
-
-void MeasureAndControl1(void)
+
+/*void MeasureAndControl1(void)
{
// This function determines the desired velocity, measures the
// actual velocity, and controls the motor with
@@ -150,7 +228,7 @@
motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1);
SetMotor1(motorValue1);
}
-
+
void MeasureAndControl2(void)
{
// This function determines the desired velocity, measures the
@@ -160,110 +238,55 @@
currentPosition2 = MeasureEncoderPosition2();
motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2);
SetMotor2(motorValue2);
-
-//------------------------------------------------------------------------------
-// Kinematic functions
-
-Matrix ComputeJ(void){
- double a = -sin(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
- double b = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1));
- double c = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
- double d = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1));
- J << a << b
- << c << d;
- return J;
-}
-
-Matrix ComputeV(void){
- V.add(1,1,vx_des); // Add desired x-velocity in V
- V.add(2,1,vy_des); // Add desired y-velocity in V
- return V;
-}
+}*/
-double IntegrateQ1(){
- q1 = q1 + (Q_set(1,1))*t; // new value for q1
- return q1;
-}
-
-double IntegrateQ2(){
- q2 = q2 + (Q_set(2,1))*t; // new value for q2
- return q2;
-}
-
-Matrix ComputeQ_set(void){
- float a = J(1,1);
- float b = J(1,2);
- float c = J(2,1);
- float d = J(2,2);
- float e = V(1,1);
- float f = V(2,1);
- float first_row = a*e + b*f;
- float second_row = c*e + d*f;
- Q_set.add(1,1,first_row);
- Q_set.add(2,1,second_row);
- return Q_set;
-}
-
-int main(){
- //led = 0;
+//------------------------------------------------------------------------------
+// MAIN
+int main()
+{
pc.baud(115200);
- pc.printf("\r\nDoet het script het?\r\n");
- // Compute Jacobian
- J = ComputeJ();
- J.print();
- pc.printf("\r\n");
- // Compute velocities
- V = ComputeV();
- V.print();
- pc.printf("\r\n");
- // Multiplying matrix J and V and storing in array Q_set
- Q_set = ComputeQ_set();
-
- // velocity to position
- q1 = IntegrateQ1();
- q2 = IntegrateQ2();
-
- /*if (q1 > ... && < ...) {
- TurnMotorsOff;
- /go to failure mode
- }
- if (q2 > ... && < ...) {
- TurnMotorsOff/go to failure mode
- }*/
-
- pc.printf("New position q1: %f, New Position q2: %f", q1, q2);
-
-}
+
+ while (true) {
+ currentPosition1 = MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
+ currentPosition2 = MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
+ J = ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
+ V = ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
+ Q_set = ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
+ q1 = IntegrateQ1(); // Compute required angle to go to desired position of end-effector
+ q2 = IntegrateQ2(); // Compute required angle to go to desired position of end-effector
+ if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
+ q1 = q1;
+ } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
+ q1 = 1.047;
+ }
+ if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
+ q2 = 0.61; // Stay at mimimum angle
+ } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
+ q2 = 1.57; // Stay at maximum angle
+ } else { // If q2 is in the right range, let calculated q2 be q2
+ q2 = q2;
+ }
+ // Determine error and add PID control
+ double errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
+ double errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
+ // Determine motorValues
+ motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
+ motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
+ // Make Motor move
+ SetMotor1(motorValue1);
+ SetMotor2(motorValue2);
-int main() {
- currentPosition1 = MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian
- currentPosition2 = MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian
- J = ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out)
- V = ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal)
- Q_set = ComputeQ_set(); // Compute Q_set (stores Q1 and Q2)
- IntegrateQ1(); // Compute required angle to go to desired position of end-effector
- IntegrateQ2(); // Compute required angle to go to desired position of end-effector
- if (q1 < 1.047) { // q1 can only be smaller than 1.047 rad
- q1 = q1;
- } else { // If value of q1 is greater than 1.047 rad, then stay at maximum angle
- q1 = 1.047;
+ // Press button to switch velocity direction
+ if (!button2.read()) {
+ //SwitchPotmeterVelocity1();
+ vy_des=vx_des;
+ vx_des=0;
+ } else {
+ vy_des=0;
+ }
+ pc.printf("vx_des = %f \t vy_des = %f \r\n", vx_des, vy_des);
+ pc.printf("Error IK1 = %f \t Error IK2 = %f \r\n", errorIK1, errorIK2);
+ pc.printf("MotorValue 1 = %f \t MotorValue 2 = %f \r\n", motorValue1, motorValue2);
+ wait(0.01);
}
- if (q2 < 0.61) { // q2 cannot be smaller than 0.61 rad
- q2 = 0.61; // Stay at mimimum angle
- } else if (q2 > 1.57) { // q2 cannot be greater than 1.57 rad
- q2 = 1.57; // Stay at maximum angle
- } else { // If q2 is in the right range, let calculated q2 be q2
- q2 = q2;
- }
- // Determine error and add PID control
- double errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle
- double errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle
- // Determine motorValues
- motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1
- motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2
- // Make Motor move
- SetMotor1(motorValue1);
- SetMotor2(motorValue2);
-
- while (true){}
}
\ No newline at end of file