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:
- 4:854aa2e7eeb2
- Parent:
- 3:d16182dd3a2a
- Child:
- 5:aca2af310419
diff -r d16182dd3a2a -r 854aa2e7eeb2 main.cpp --- a/main.cpp Thu Nov 01 20:15:22 2018 +0000 +++ b/main.cpp Thu Nov 01 20:50:17 2018 +0000 @@ -18,12 +18,14 @@ // Tickers Ticker potmeterTicker; - +Ticker allesineenticker; +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] +// ^ waarom? int a = 0; //Inverse Kinematics @@ -33,12 +35,15 @@ 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] +// ^ start buiten bereik, waarom? 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 +volatile double errorIK1 = 0.0; +volatile double errorIK2 = 0.0; // Motor Control volatile double potMeterPosition1 = 0.0; @@ -61,7 +66,8 @@ //return vx_des; } -double SwitchPotmeterVelocity1() { //Button has been pressed, +double SwitchPotmeterVelocity1() //Button has been pressed, +{ vy_des = vx_des; return vy_des; } @@ -119,7 +125,7 @@ double errorIK1 = q1 - currentPosition1; return errorIK1; } - + double ErrorInverseKinematics2() { double errorIK2 = q2 - currentPosition2; @@ -239,57 +245,66 @@ motorValue2 = FeedbackControl2(potMeterPosition2 - currentPosition2); SetMotor2(motorValue2); }*/ +void AllesinEen() //HIER GAAT IETS MIS!! +{ + 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 + 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; + } else { + vy_des=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", Q_set(1,1), Q_set(2,1)); +} //------------------------------------------------------------------------------ // MAIN int main() { pc.baud(115200); - potmeterTicker.attach(GetPotMeterVelocity1, 0.01); - + potmeterTicker.attach(GetPotMeterVelocity1, 0.01f); + allesineenticker.attach(AllesinEen, 0.01f); + printTicker.attach(printen, 0.5f); + 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); - pc.printf("q1 Qset = %f \t q2 Qset = %f \r\n", Q_set(1,1), Q_set(2,1)); - wait(0.01); + //wait(0.01); } } \ No newline at end of file