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:
- 8:3d2228402c71
- Parent:
- 7:fb3da4df4269
- Child:
- 9:7443c37f0f7b
diff -r fb3da4df4269 -r 3d2228402c71 main.cpp --- a/main.cpp Tue Nov 06 16:12:17 2018 +0000 +++ b/main.cpp Tue Nov 06 20:25:40 2018 +0000 @@ -8,7 +8,7 @@ DigitalOut motor1Direction(D7); DigitalOut motor2Direction(D4); DigitalOut Led(LED_GREEN); -DigitalIn button2(SW3); //dit is nog button op mbed bor +DigitalIn button2(SW3); FastPWM motor1PWM(D6); FastPWM motor2PWM(D5); AnalogIn potMeter1(A4); @@ -18,25 +18,23 @@ // Tickers Ticker potmeterTicker; -Ticker allesineenticker; +Ticker measurecontrolTicker; 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; +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] 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! +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] -// ^ start buiten bereik, waarom? -float t = 0.01; // Time interval [s] SHOULD BE REPLACED SAMPLING TIJD!! + +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; @@ -57,10 +55,10 @@ //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 Kp = 0.34; +volatile float Ki = 0.0; volatile float Kd = 0.0; -volatile float Ts = 0.01; // SAMPLING TIJD!! +volatile float Ts = 0.02; //------------------------------------------------------------------------------ @@ -68,9 +66,7 @@ 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; + vx_des = 4.0*3.14*potMeter1In - 2.0*3.14; } float SwitchPotmeterVelocity1() //Button has been pressed, @@ -127,6 +123,13 @@ } //------------------------------------------------------------------------------ // 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] { @@ -216,28 +219,7 @@ } } -/*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!! +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 @@ -246,34 +228,35 @@ 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; - } + 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()) { - if (!button2.read()) { //button2 blijven indrukken - //SwitchPotmeterVelocity1(); + if (!button2.read()) { //keep pressing button2 vy_des=vx_des; vx_des=0.0; } else { @@ -281,25 +264,24 @@ } } -void printen() +/*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.01); - allesineenticker.attach(AllesinEen, 0.01); - printTicker.attach(printen, 0.5); + potmeterTicker.attach(GetPotMeterVelocity1, 0.02); + measurecontrolTicker.attach(MeasureAndInverseK, 0.01); + /*printTicker.attach(printen, 0.5);*/ while (true) { Led = !Led; wait(0.5); - //wait(0.01); } } \ No newline at end of file