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:
- nicollevanrijswijk
- Date:
- 2018-11-03
- Revision:
- 6:a6f79f31767b
- Parent:
- 5:aca2af310419
- Child:
- 7:fb3da4df4269
File content as of revision 6:a6f79f31767b:
#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; Ticker allesineenticker; Ticker printTicker; //------------------------------------------------------------------------------ //Global Variables float currentPosition1 = 0.1; // Starting position of motor 1 [rad] float currentPosition2 = -(0.5*3.14); // Starting position of motor 2 wrt motor 1 [rad] // ^ waarom? int a = 0; //Inverse Kinematics const float L0 = 0.1; // Horizontal length from base to first joint [m] NAMETEN!! const float L1 = 0.3; // Length link 1 [m] NAMETEN!! const float L2 = 0.3; // Length link 2 [m] NAMETEN!! const float L3 = 0.1; // Vertical length from base to first joint [m] NAMETEN! 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] // ^ start buiten bereik, waarom? float t = 0.01; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!! 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 = 4.0; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot volatile float Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld volatile float Kd = 0.0; volatile float Ts = 0.01; // SAMPLING TIJD!! //------------------------------------------------------------------------------ // 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; // Reference value y, scaled to -0.25 to 0.25 revolutions //vx_des = 0.5; //return vx_des; } float SwitchPotmeterVelocity1() //Button has been pressed, { vy_des = vx_des; return vy_des; } //------------------------------------------------------------------------------ // KINEMATIC FUNCTIONS void ComputeJ(void) { J1 = -sin(currentPosition1)/(L1*cos(currentPosition1)*sin(currentPosition2)-L1*cos(currentPosition2)*sin(currentPosition1)); J2 = cos(currentPosition2)/(L1*cos(currentPosition1)*sin(currentPosition2)-L1*cos(currentPosition2)*sin(currentPosition1)); J3 = (L1*sin(currentPosition1)+L2*sin(currentPosition2))/(L1*L2*cos(currentPosition1)*sin(currentPosition2)-L1*L2*cos(currentPosition2)*sin(currentPosition1)); J4 = -(L1*cos(currentPosition1)+L2*cos(currentPosition2))/(L1*L2*cos(currentPosition1)*sin(currentPosition2)-L1*L2*cos(currentPosition2)*sin(currentPosition1)); } 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 //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 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); }*/ void AllesinEen() //HIER GAAT IETS MIS!! { 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 (q > 0.0) { // q1 cannot be greater than 0 q = 0.0; } else if (q < - 1.047) { // q1 cannot be smaller than 60 degrees q1 = -1.047; } else { // q1 can be computed q1 between intervals q1 = q1; } 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 // 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.0; } else { vy_des=0.0; } } void printen() { 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.01); allesineenticker.attach(AllesinEen, 0.01); printTicker.attach(printen, 0.5); while (true) { Led = !Led; wait(0.5); //wait(0.01); } }