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-06
- Revision:
- 8:3d2228402c71
- Parent:
- 7:fb3da4df4269
- Child:
- 9:7443c37f0f7b
File content as of revision 8:3d2228402c71:
#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); 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; Ticker measurecontrolTicker; Ticker printTicker; //------------------------------------------------------------------------------ //Global Variables volatile float currentPosition1 = 0.1; // Starting position of motor 1 [rad] volatile float currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad] //Inverse Kinematics const float L0 = 0.1; // Horizontal length from base to first joint [m] const float L1 = 0.3; // Length link 1 [m] const float L2 = 0.3; // Length link 2 [m] const float L3 = 0.1; // Vertical length from base to first joint [m] volatile float q1 = 0.1; // Starting reference angle of first link [rad] volatile float q2 = -(0.5*3.12); // Starting reference angle of second link wrt first link [rad] float t = 0.02; // Time interval [s] volatile float vx_des = 0.0; // Starting velocity in x-direction [rad/s] volatile float vy_des = 0.0; // Starting velocity in y-direction [rad/s] volatile float errorIK1 = 0.0; volatile float errorIK2 = 0.0; volatile float J1 = 0.0; volatile float J2 = 0.0; volatile float J3 = 0.0; volatile float J4 = 0.0; volatile float V1 = 0.0; volatile float V2 = 0.0; volatile float Q1 = 0.0; volatile float Q2 = 0.0; // Motor Control volatile float potMeterPosition1 = 0.0; int counts1i = 0; int counts2i = 0; //volatile float potMeterPosition2 = 0.0; volatile float motorValue1 = 0.01; volatile float motorValue2 = 0.01; volatile float Kp = 0.34; volatile float Ki = 0.0; volatile float Kd = 0.0; volatile float Ts = 0.02; //------------------------------------------------------------------------------ // Potmeter values TO DETERMINE VX_DES, VY_DES void GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION { float potMeter1In = potMeter1.read(); vx_des = 4.0*3.14*potMeter1In - 2.0*3.14; } float SwitchPotmeterVelocity1() //Button has been pressed, { vy_des = vx_des; return vy_des; } //------------------------------------------------------------------------------ // KINEMATIC FUNCTIONS void ComputeJ(void) { J1 = -sin(q1)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1)); J2 = cos(q2)/(L1*cos(q1)*sin(q2)-L1*cos(q2)*sin(q1)); J3 = (L1*sin(q1)+L2*sin(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1)); J4 = -(L1*cos(q1)+L2*cos(q2))/(L1*L2*cos(q1)*sin(q2)-L1*L2*cos(q2)*sin(q1)); } void ComputeV(void) { V1 = vx_des; V2 = vy_des; } float IntegrateQ1() { q1 = q1 + (Q1*t); // new value for q1 return q1; } float IntegrateQ2() { q2 = q2 + (Q2*t); // new value for q2 return q2; } void ComputeQ_set() { Q1 = (J1*V1) + (J2*V2); Q2 = (J3*V1) + (J4*V2); } float ErrorInverseKinematics1() { float errorIK1 = q1 - currentPosition1; return errorIK1; } float ErrorInverseKinematics2() { float errorIK2 = q2 - currentPosition2; return errorIK2; } //------------------------------------------------------------------------------ // MOTOR PART // Failure function void TurnMotorsOff() // Turn motors off immediately { motor1PWM = 0; motor2PWM = 0; } //Encoder Posities void MeasureEncoderPosition1() // Read current position of motor 1, returns value in [rad] { counts1i = Encoder2.getPulses(); float counts1 = counts1i*1.0f; currentPosition1 = (counts1/8400)*6.28; //Rotational position in radians } void MeasureEncoderPosition2() // Read current postion of motor 2, returns value in [rad] { counts2i = Encoder2.getPulses(); float counts2 = counts2i*1.0f; currentPosition2 = (counts2/8400)*6.28; //Rotational position in radians } float FeedbackControl1(float Error1) { static float Error_integral1 = 0; static float Error_prev1 = Error1; // Proportional part: float u_k1 = Kp * Error1; // Integral part: Error_integral1 = Error_integral1 + Error1 * Ts; float u_i1 = Ki * Error_integral1; // Derivative part: float Error_derivative1 = (Error1 - Error_prev1)/Ts; float u_d1 = Kd * Error_derivative1; Error_prev1 = Error1; // Sum all parts and return it return u_k1 + u_i1 + u_d1; //motorValue } float FeedbackControl2(float Error2) { static float Error_integral2 = 0; static float Error_prev2 = Error2; // Proportional part: float u_k2 = Kp * Error2; // Integral part: Error_integral2 = Error_integral2 + Error2 * Ts; float u_i2 = Ki * Error_integral2; // Derivative part: float Error_derivative2 = (Error2 - Error_prev2)/Ts; float u_d2 = Kd * Error_derivative2; Error_prev2 = Error2; // Sum all parts and return it return u_k2 + u_i2 + u_d2; //motorValue } void SetMotor1(float 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(float 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 MeasureAndInverseK() { MeasureEncoderPosition1(); // You want to know the current angle of the motors to get the right Jacobian MeasureEncoderPosition2(); // You want to know the current angle of the motors to get the right Jacobian ComputeJ(); // Compute matrix J (inverse Jacobian, and Twist is already left out) ComputeV(); // Compute matrix V (stores the desired velocities obtained from the EMG signal) 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 errorIK1 = ErrorInverseKinematics1(); // Determine difference between current angle motor 1 and desired angle errorIK2 = ErrorInverseKinematics2(); // Determine difference between current angle motor 2 and desired angle // Determine motorValues motorValue1 = FeedbackControl1(errorIK1); // Calculate motorValue1 motorValue2 = FeedbackControl2(errorIK2); // Calculate motorValue2 if (errorIK1<0.5); { TurnMotorsOff(); } // Make Motor move SetMotor1(motorValue1); SetMotor2(motorValue2); // Press button to switch velocity direction if (!button2.read()) { //keep pressing button2 vy_des=vx_des; vx_des=0.0; } else { vy_des=0.0; } } /*void PrintValues() { 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); pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q1, Q2); }*/ //------------------------------------------------------------------------------ // MAIN int main() { pc.baud(115200); potmeterTicker.attach(GetPotMeterVelocity1, 0.02); measurecontrolTicker.attach(MeasureAndInverseK, 0.01); /*printTicker.attach(printen, 0.5);*/ while (true) { Led = !Led; wait(0.5); } }