Revision 5:52badb8ee317, committed 2017-11-03

Comitter:

RoyvZ

Date:

Fri Nov 03 06:39:02 2017 +0000

Parent:

4:dfed3b98ffb1

Commit message:

MBed 2 -> af;

Changed in this revision

diff -r dfed3b98ffb1 -r 52badb8ee317 main.cpp
--- 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