DemoState with IK and MotorControl and input vx_des and vy_des with potmeter1 and button2
Dependencies: FastPWM MODSERIAL Matrix MatrixMath mbed QEI
main.cpp
- Committer:
- 1856413
- Date:
- 2018-11-01
- Revision:
- 2:638c6155d0af
- Parent:
- 1:2219a519e2bf
- Child:
- 3:d16182dd3a2a
File content as of revision 2:638c6155d0af:
#include "mbed.h" #include "FastPWM.h" #include "MODSERIAL.h" #include "Matrix.h" #include "QEI.h" #include <math.h> MODSERIAL pc(USBTX, USBRX); DigitalOut motor1Direction(D7); DigitalOut motor2Direction(D4); DigitalOut Led(LED_GREEN); DigitalIn button2(SW3); //dit is nog button op mbed bor FastPWM motor1PWM(D6); FastPWM motor2PWM(D5); AnalogIn potMeter1(A4); AnalogIn potMeter2(A5); QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING); QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING); // Tickers Ticker potmeterTicker; //------------------------------------------------------------------------------ //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!! const double L2 = 0.3; // Length link 2 [m] NAMETEN!! const double L3 = 0.1; // Vertical length from base to first joint [m] NAMETEN! 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; volatile double motorValue1 = 0.01; volatile double motorValue2 = 0.01; volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot 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 void GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION { double potMeter1In = potMeter1.read(); //vx_des = 0.5*3.14*potMeter1In - 0.25*3.14 ; // Reference value y, scaled to -0.25 to 0.25 revolutions vx_des = 3.7; //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 ErrorInverseKinematics2() { 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; } 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; // Proportional part: double u_k1 = Kp * Error1; // Integral part: Error_integral1 = Error_integral1 + Error1 * Ts; double u_i1 = Ki * Error_integral1; // 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; // Proportional part: double u_k2 = Kp * Error2; // Integral part: Error_integral2 = Error_integral2 + Error2 * Ts; double u_i2 = Ki * Error_integral2; // 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 // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range // 0 = clockwise motor direction // 1 = counterclockwise motor direction if (motorValue1 >=0) { motor1Direction=0; } else { motor1Direction=1; } if (fabs(motorValue1)>1) { motor1PWM = 1; } else { motor1PWM = fabs(motorValue1); } } void SetMotor2(double motorValue2) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range // 0 = counterclockwise motor direction // 1 = clockwise motor direction if (motorValue2 >=0) { motor2Direction=1; } else { motor2Direction=0; } if (fabs(motorValue2)>1) { motor2PWM = 1; } else { motor2PWM = fabs(motorValue2); } } /*void MeasureAndControl1(void) { // This function determines the desired velocity, measures the // actual velocity, and controls the motor with // a simple Feedback controller. Call this from a Ticker. MeterPosition1 = q1; currentPosition1 = MeasureEncoderPosition1(); motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1); SetMotor1(motorValue1); } void MeasureAndControl2(void) { // This function determines the desired velocity, measures the // actual velocity, and controls the motor with // a simple Feedback controller. Call this from a Ticker. potMeterPosition2 = q2; currentPosition2 = MeasureEncoderPosition2(); motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2); SetMotor2(motorValue2); }*/ //------------------------------------------------------------------------------ // MAIN int main() { pc.baud(115200); potmeterTicker.attach(GetPotMeterVelocity1, 0.01); 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); // Press button to switch velocity direction //if (!button2.read()) { if (!button2.read()) { //button2 blijven indrukken //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); } }