![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Voor matthijs
Dependencies: Encoder MODSERIAL Matrix MatrixMath QEI biquad-master mbed
Fork of frdm_gpio1 by
Diff: main.cpp
- Revision:
- 5:52badb8ee317
- Parent:
- 4:dfed3b98ffb1
--- a/main.cpp Wed Nov 01 15:08:27 2017 +0000 +++ b/main.cpp Fri Nov 03 06:39:02 2017 +0000 @@ -1,226 +1,181 @@ -/** -* Demo program for BiQuad and BiQuadChain classes -* author: T.J.W. Lankhorst <t.j.w.lankhorst@student.utwente.nl> and Matthijs and Roy and Dion -*/ #include "mbed.h" -//#include "HIDScope.h" -#include <stdlib.h> -#include <iostream> -#include <iomanip> -#include <complex> -#include <vector> -#include "BiQuad.h" -#include "Matrix.h" -#include "MatrixMath.h" -#include "MODSERIAL.h" #include "encoder.h" -AnalogIn emg0( A0 ); -AnalogIn emg1( A1 ); -DigitalIn button1(D11); -Encoder encoder1(D13,D12); -Encoder encoder2(D13,D12); //moet nog even aangepast worden +PwmOut RightServoVel(D10); +PwmOut LeftServoVel(D11); -AnalogIn potMeterIn1(A1); - +PwmOut TurningMotorVel(D4); +DigitalOut TurningMotorDir(D5); -DigitalOut motorDirection(D4); -PwmOut motorSpeed(D5); - -MODSERIAL pc(USBTX, USBRX); +Ticker ServoTick; +Ticker TurningTick; -Ticker sample_timer; -Ticker cal_timer; -Ticker setpoint_timer; -Ticker m1_Ticker; -Ticker SP_m1_Ticker; -//HIDScope scope( 3 ); -DigitalOut led(LED1); -DigitalOut led2(LED_GREEN); -const int size = 100; -vector<double> S(size,0); -double meanSum = 0; +DigitalOut ledr(LED_RED); +DigitalOut ledg(LED_GREEN); +DigitalOut ledb(LED_BLUE); -double maxsignal = 0; +DigitalIn CcwTurn(D0); +DigitalIn CwTurn(D1); +InterruptIn ServoSwitch(D2); +DigitalIn qGreenPos(D6); //Uitzoeken of we de Mbed kunnen linken -double emgX = 0; -double emgY = 1; +int maxPulses = 6000; +int halfPulses = maxPulses/2; +int backPulses = maxPulses*4/7 ; +bool ToTurnOrNotToTurn; -double L0 = 0.123; -double L1 = 0.37; -double L2 = 0.41; -double q1 = encoder1.getPosition()/131; //Calibreren nog toevoegen -double q2 = encoder2.getPosition()/131; //Calibreren mist nog -double Periode = 0.002; //1000 is het aantal Hz +Encoder encoder3(D13,D12); +bool CcwRot = true; +bool CwRot = !CcwRot; -float M1_KP = 2.5; -float M1_KI = 1.0; -const float M1_TS = 0.01; -const float SP_TS = 0.01; -const float RAD_PER_PULSE = 0.000119; -float m1_err_int = 0; -int motorD = 0; -float motor1 = 0; -float reference = 0; -float position = 0; -int outOfEncod = 0; -int KP_changer = 1; -float RotSpeed = 0; +enum TurningStates{CcwTurning,CwTurning,NoTurning}; +enum ServoStates{Open, Close}; + +TurningStates currentState_T = NoTurning; +ServoStates currentState_S = Close; +ServoStates previousState_S = Close; -//Making matrices globaly -Matrix JAPPAPP(2,2); -Matrix qdot(2,1); -Matrix Vdes(2,1); -Matrix qsetpoint(2,1); +int j = 0; +int count = 0; -//Filter toevoegen, bestaande uit notch, high- en lowpass filter -BiQuadChain Notch; -BiQuadChain Notch50; -BiQuadChain bqcLP; -BiQuadChain bqc; -//50 Hz Notch filter, for radiation from surroundings - -//Notch filter chain for 100 Hz -BiQuad bqNotch1( 9.75180e-01, -1.85510e+00, 9.75218e-01, -1.87865e+00, 9.75178e-01 ); -BiQuad bqNotch2( 1.00000e+00, -1.90228e+00, 1.00004e+00, -1.88308e+00, 9.87097e-01 ); -BiQuad bqNotch3( 1.00000e+00, -1.90219e+00, 9.99925e-01, -1.89724e+00, 9.87929e-01 ); - - -//Notch filter chain for 50 Hz -BiQuad bq1( 9.50972e-01, -1.53921e+00, 9.51003e-01, -1.57886e+00, 9.50957e-01 ); -BiQuad bq2( 1.00000e+00, -1.61851e+00, 9.99979e-01, -1.57185e+00, 9.74451e-01 ); -BiQuad bq3( 1.00000e+00, -1.61855e+00, 9.99988e-01, -1.62358e+00, 9.75921e-01 ); - -//2Hz Highpass -BiQuad Highpass1(9.82385e-01, -1.96477e+00, 9.82385e-01, -1.96446e+00, 9.65081e-01); - -//80 Hz Lowpass -BiQuad bqLP1( 5.78904e-02, 5.78898e-02, 0.00000e+00, -2.90527e-01, 0.00000e+00 ); -BiQuad bqLP2( 1.00000e+00, 2.00001e+00, 1.00001e+00, -7.53537e-01, 4.06308e-01 ); - - - -//450 Hz Lowpass filter, to remove any noise (sample frequency is only 500 Hz, but still...) -//BiQuad Lowpass450(8.00592e-01, 1.60118e+00, 8.00592e-01, 1.56102e+00, 6.41352e-01); - -float PI( float e, const float Kp, const float Ki, float Ts, float &e_int ) -{ - e_int += Ts * e; // e_int is changed globally because it’s ’by reference’ (&) - if (fabs(Kp * e + Ki * e_int) < 1) { - return fabs(Kp * e + Ki * e_int); - } else { - return 1; +void ProcessStateMachineTurning(){ + ToTurnOrNotToTurn = (qGreenPos) ? true : false; //Determening if the arm goes underneath the base + if(ToTurnOrNotToTurn){ + if(CcwTurn && CwTurn){ + currentState_T = NoTurning; //No turning when both buttons are pressed + } else if(CcwTurn){ + currentState_T = CcwTurning; //Turning Ccw + } else if (CwTurn){ + currentState_T = CwTurning; //Turning Cw + } else { + currentState_T = NoTurning; //No buttons pressed No turning + } + } + else { + currentState_T = NoTurning; + } + + switch (currentState_T){ + case CcwTurning: + if(encoder3.getPosition() > 0) { //Turning Counter clock wise + TurningMotorVel.write(0.1f); + TurningMotorDir.write(CcwRot); + } + else{ + while(encoder3.getPosition() < (backPulses)){ + TurningMotorVel.write(0.1f); //Turning Back in Cw + TurningMotorDir.write(CwRot); + } + } + break; + + case CwTurning: + if (encoder3.getPosition() < maxPulses){ //Turning clock wise + TurningMotorVel.write(0.1f); //Turning in Cw + TurningMotorDir.write(CwRot); + } + else{ + while(encoder3.getPosition() > (maxPulses - backPulses)){ + TurningMotorVel.write(0.1f); //Turning Back in Ccw + TurningMotorDir.write(CcwRot); + } + } + break; + + case NoTurning: + TurningMotorVel.write(0.0f); + break; } } -// Next task, measure the error and apply the output to the plant -void m1_Controller() -{ - reference = 5 * potMeterIn1.read(); - //reference = qsetpoint(1,1); - outOfEncod = encoder1.getPosition(); - position = RAD_PER_PULSE * outOfEncod; - float ref_pos = reference - position; // Don’t use magic numbers! - //motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int ); - - if (ref_pos <= 0) { // Don’t use magic numbers! - motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int ); - } else { - motor1 = PI(ref_pos, M1_KP, M1_KI, M1_TS, m1_err_int ); - } - if (ref_pos <= 0) { - //float motor1DirectionPin1 = 1; - motorD=1; - } else { - //float motor1DirectionPin1 = 0; - motorD=0; - } - motorDirection.write(motorD); - motorSpeed.write(motor1); -} -double findRMS(vector<double> array) -{ - int i; - double sumsquared = 0.000; - double RMS; - //sumsquared = 0; - for (i = 0; i < size; i++) { - sumsquared = sumsquared + array.at(i)*array.at(i); +void ProcessStateMachineServo(void) { +/** +State machine for the servo motors, motor response dependent on the current state and the amount of milliseconds of activation. +The entire function must be sampled at 2000 Hz, since it counts half milliseconds. This is required since the servos respond to half millisecond actuation. +For further information on the functioning of the servo motors; see paragraph ... in report 'Design of a Jenga playing robot' by group 2. +**/ + switch (currentState_S) + { + case Close: + if(j <= 35){ + RightServoVel.write(0); LeftServoVel.write(0); + j++; + } + else if (j == 36){ + RightServoVel.write(1); LeftServoVel.write(0); + j++; + } + else if (j == 37){ + RightServoVel.write(1); LeftServoVel.write(0); + j++; + } + else if (j == 38){ + RightServoVel.write(1); LeftServoVel.write(0); + j++; + } + else if (j == 39){ + RightServoVel.write(1); LeftServoVel.write(1); + j = 0; + } + previousState_S = Close; + break; + + case Open: + if(j <= 35){ + RightServoVel.write(0); LeftServoVel.write(0); + j++; + } + else if (j == 36){ + RightServoVel.write(0); LeftServoVel.write(0); + j++; + } + else if (j == 37){ + RightServoVel.write(0); LeftServoVel.write(1); + j++; + } + else if (j == 38){ + RightServoVel.write(1); LeftServoVel.write(1); + j++; + } + else if (j == 39){ + RightServoVel.write(1); LeftServoVel.write(1); + j = 0; + } + previousState_S = Open; + break; } - RMS = sqrt((double(1)/size)*(sumsquared)); - return RMS; } - - -void sample() -{ - S.erase(S.begin()); - double b = bqc.step(emg0.read()-0.4542); - S.push_back(b); - emgX = findRMS(S); - - led = !led; - - //motorSpeed.write(0.38080*(emgX/maxsignal)); - //motorDirection.write(1); +void ChangingState(){ + currentState_S = previousState_S; } - - -void qSetpointSet() -{ - // Fill Matrix with data. - JAPPAPP(1,1) = -(L0 - L0*sin(q1) + L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); - JAPPAPP(1,2) = -(L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); - JAPPAPP(2,1) = (L1*sin(q1) - L2*sin(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); - JAPPAPP(2,2) = (L1*cos(q1) + L2*cos(q2))/(L1*L1*cos(q1)*sin(q1) + L0*L1*cos(q1) + L0*L2*cos(q2) - L0*L1*cos(q1)*sin(q1) - L0*L2*cos(q2)*sin(q1) - L1*L2*cos(q1)*sin(q2)); - - // Fill Matrix with data. - Vdes(1,1) = emgX; //Goede code nog toevoegen, Vx - Vdes(2,1) = emgY; //goede code nog toevoegen, Vy - - qdot = JAPPAPP*Vdes; - - qsetpoint(1,1) = q1 + qdot(1,1)*Periode; - qsetpoint(2,1) = q2 + qdot(2,1)*Periode; - - //motorSpeed.write(qsetpoint(1,1)); - //motorDirection.write(1); -} - + int main() { - pc.baud(115200); - - //Constructing the notch filter chain - Notch.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ); - Notch50.add( &bq1 ).add( &bq2 ).add( &bq3 ); - bqcLP.add( &bqLP1 ).add( &bqLP2 ); - bqc.add( &bqNotch1 ).add( &bqNotch2 ).add( &bqNotch3 ).add( &bq1 ).add( &bq2 ).add( &bq3 ).add( &bqLP1 ).add( &bqLP2 ); - /**Attach the 'sample' function to the timer 'sample_timer'. - * this ensures that 'sample' is executed every... 0.001 seconds = 1000 Hz - */ - sample_timer.attach(&sample, 0.002); - setpoint_timer.attach(&qSetpointSet, 0.002); - led2 = 1; - /*empty loop, sample() is executed periodically*/ - m1_Ticker.attach( &m1_Controller, M1_TS ); - - m1_Controller(); - - while(true) { - pc.printf("%f \t",emgX); - pc.printf("%f \t",qdot(1,1)); - pc.printf("%0.8f \t",qsetpoint(1,1)); - pc.printf("%f \t",encoder1.getPosition()); - pc.printf("%f \t",motorD); - pc.printf("%f \t",emgX); - pc.printf("%f \r\n",reference); - + ledr = 1; + ledg = 1; + ledb = 1; + + ServoSwitch.rise(&ChangingState); + + ServoTick.attach(ProcessStateMachineServo,0.0005f); + TurningTick.attach(ProcessStateMachineTurning, 0.001f); + + encoder3.setPosition(halfPulses); + + while (true){ + if (!CcwTurn){ + ledr = 0; ledg = 1; ledb = 1; // ROOD + } else if(!CwTurn){ + ledr = 1; ledg = 0; ledb = 1; //GROEN + } else if(!ServoSwitch){ + ledr = 1; ledg = 1; ledb = 0; //BLAUW + } else { + ledr = 1; ledg = 1; ledb = 1; + } } - } \ No newline at end of file