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:
- 2:638c6155d0af
- Parent:
- 1:2219a519e2bf
- Child:
- 3:d16182dd3a2a
diff -r 2219a519e2bf -r 638c6155d0af main.cpp --- a/main.cpp Thu Nov 01 19:24:28 2018 +0000 +++ b/main.cpp Thu Nov 01 19:50:29 2018 +0000 @@ -8,7 +8,7 @@ DigitalOut motor1Direction(D7); DigitalOut motor2Direction(D4); DigitalOut Led(LED_GREEN); -DigitalIn button2(SW3); +DigitalIn button2(SW3); //dit is nog button op mbed bor FastPWM motor1PWM(D6); FastPWM motor2PWM(D5); AnalogIn potMeter1(A4); @@ -17,8 +17,7 @@ QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING); // Tickers -Ticker startMotor; -Ticker printTicker; +Ticker potmeterTicker; //------------------------------------------------------------------------------ //Global Variables @@ -43,7 +42,7 @@ // Motor Control volatile double potMeterPosition1 = 0.0; -volatile double potMeterPosition2 = 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 @@ -54,11 +53,12 @@ //------------------------------------------------------------------------------ // Potmeter values TO DETERMINE VX_DES, VY_DES -double GetPotMeterVelocity1() //Potmeter standard to control X-DIRECTION +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 - return vx_des; + //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, @@ -223,7 +223,7 @@ // This function determines the desired velocity, measures the // actual velocity, and controls the motor with // a simple Feedback controller. Call this from a Ticker. - potMeterPosition1 = q1; + MeterPosition1 = q1; currentPosition1 = MeasureEncoderPosition1(); motorValue1 = FeedbackControl1(potMeterPosition1 - currentPosition1); SetMotor1(motorValue1); @@ -245,6 +245,7 @@ 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 @@ -277,7 +278,8 @@ SetMotor2(motorValue2); // Press button to switch velocity direction - if (!button2.read()) { + //if (!button2.read()) { + if (!button2.read()) { //button2 blijven indrukken //SwitchPotmeterVelocity1(); vy_des=vx_des; vx_des=0;