Brighton de Jong
/
Turning_Motor_V6
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL Matrix QEI biquadFilter mbed
Fork of
Filter
by 
Jurriën Bos
main.cpp
- Committer:
- Brighton_the_robot
- Date:
- 2018-10-31
- Revision:
- 8:b6b09226a421
- Parent:
- 7:c5c648898881
File content as of revision 8:b6b09226a421:
#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "BiQuad.h"
MODSERIAL pc(USBTX, USBRX);
DigitalOut DirectionPin1(D4);
DigitalOut DirectionPin2(D7);
PwmOut PwmPin1(D5);
PwmOut PwmPin2(D6);
DigitalIn Knop1(D2);
DigitalIn Knop2(D3);
AnalogIn pot1 (A5);
AnalogIn pot2 (A4);
AnalogIn emg0( A0 );
AnalogIn emg1( A1 );
AnalogIn emg2( A2 );
AnalogIn emg3( A3 );
QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
//DigitalOut LED(LED_RED);
Ticker StateTicker;
Ticker printTicker;
HIDScope scope(4);
BiQuadChain bqc1;
BiQuadChain bqc2;
BiQuadChain bqc3;
BiQuadChain bqc4;
BiQuadChain bqc5;
BiQuadChain bqc6;
BiQuadChain bqc7;
BiQuadChain bqc8;
BiQuad BqNotch1_1( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
BiQuad BqNotch2_1( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad BqNotch1_2( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
BiQuad BqNotch2_2( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad BqNotch1_3( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
BiQuad BqNotch2_3( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad BqNotch1_4( 9.65081e-01, -1.56203e+00, 9.65081e-01,-1.56858e+00, 9.64241e-01 );
BiQuad BqNotch2_4( 1.00000e+00, -1.61855e+00, 1.00000e+00 ,-1.61100e+00, 9.65922e-01);
BiQuad BqHP1( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad BqHP2( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad BqHP3( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad BqHP4( 9.86760e-01, -1.97352e+00, 9.86760e-01, -1.97334e+00, 9.73695e-01 );
BiQuad BqLP1( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
BiQuad BqLP2( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
BiQuad BqLP3( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
BiQuad BqLP4( 8.76555e-05, 1.75311e-04, 8.76555e-05, -1.97334e+00, 9.73695e-01 );
volatile float Bicep_Right = 0.0;
volatile float Bicep_Left = 0.0;
volatile float Tricep_Right = 0.0;
volatile float Tricep_Left = 0.0;
volatile const float maxVelocity = 8.4; // in rad/s
volatile const double pi = 3.14159265358979;
volatile float referenceVelocity1 = 0.5; //dit is de gecentreerde waarde en dus de nulstand
volatile float referenceVelocity2 = 0.5;
enum states{Calibration, Homing, Function};
volatile states Active_State = Calibration;
volatile int counts1 ;
volatile int counts2 ;
volatile float rad_m1;
volatile float rad_m2;
volatile float q_1;
volatile float q_2;
volatile float r_1;
volatile float r_2;
volatile const float r_3 = 2.0; //
volatile float FilterHP_Bi_R;
volatile float Filterabs_Bi_R;
volatile float Filtered_Bi_R;
volatile float FilterHP_Bi_L;
volatile float Filterabs_Bi_L;
volatile float Filtered_Bi_L;
volatile float FilterHP_Tri_R;
volatile float Filterabs_Tri_R;
volatile float Filtered_Tri_R;
volatile float FilterHP_Tri_L;
volatile float Filterabs_Tri_L;
volatile float Filtered_Tri_L;
void filter()
{
FilterHP_Bi_R = bqc1.step( emg0.read() );
Filterabs_Bi_R = fabs(FilterHP_Bi_R);
Filtered_Bi_R = bqc2.step( Filterabs_Bi_R );
FilterHP_Bi_L = bqc3.step( emg1.read() );
Filterabs_Bi_L = fabs(FilterHP_Bi_L);
Filtered_Bi_L = bqc4.step( Filterabs_Bi_L );
FilterHP_Tri_R = bqc5.step( emg2.read() );
Filterabs_Tri_R = fabs(FilterHP_Tri_R);
Filtered_Tri_R = bqc6.step( Filterabs_Tri_R );
FilterHP_Tri_L = bqc7.step( emg3.read() );
Filterabs_Tri_L = fabs(FilterHP_Tri_L);
Filtered_Tri_L = bqc8.step( Filterabs_Tri_L );
}
void Encoding()
{
counts1 = Encoder1.getPulses();
counts2 = Encoder2.getPulses();
// Hier gaat iets fout waardoor het 0 wordt!!!
rad_m1 = ((2.0*pi)/32.0)* (float)counts1;
rad_m2 = ((2.0*pi)/32.0)* (float)counts2;
// pc.printf("%f & %f ....\n",rad_m1, rad_m2);
}
void EMG_Read()
{
Bicep_Right = emg0.read();
Bicep_Left = emg1.read();
Tricep_Right = emg2.read();
Tricep_Left = emg3.read();
}
void sample()
{
scope.set(0, Filtered_Bi_R );
scope.set(1, Filtered_Bi_L );
scope.set(2, Filtered_Tri_R );
scope.set(3, Filtered_Tri_L );
scope.send();
}
void velocity1()
{
if (pot1.read()>0.5f)
{
// Clockwise rotation
referenceVelocity1 = (pot1.read()-0.5f) * 2.0f;
}
else if (pot1.read() == 0.5f)
{
referenceVelocity1 = pot1.read() * 0.0f;
}
else if (pot1.read() < 0.5f)
{
// Counterclockwise rotation
referenceVelocity1 = 2.0f * (pot1.read()-0.5f) ;
}
}
void velocity2()
{
if (pot2.read()>0.5f)
{
// Clockwise rotation
referenceVelocity2 = (pot2.read()-0.5f) * 2.0f;
}
else if (pot2.read() == 0.5f)
{
referenceVelocity2 = pot2.read() * 0.0f;
}
else if (pot2.read() < 0.5f)
{
// Counterclockwise rotation
referenceVelocity2 = 2.0f * (pot2.read()-0.5f) ;
}
}
void motor1()
{
float u = referenceVelocity1;
DirectionPin1 = u < 0.0f;
PwmPin1 = fabs(u);
}
void motor2()
{
float u = referenceVelocity2;
DirectionPin2 = u > 0.0f;
PwmPin2 = fabs(u);
}
void Printing()
{
float v1 = fabs(referenceVelocity1) * maxVelocity;
float v2 = fabs(referenceVelocity2) * maxVelocity;
//eventueel nog counts -> rad/s
//pc.printf("%f \n %f snelheid Motor1 \n %f snelheid Motor2 \n", Bicep_Right,v1,v2);
pc.printf("%i %i \n",counts1,counts2);
}
void inverse_kinematics()
{
float JacPs [2][2];
JacPs[0][0] = 2.0;
JacPs[0][1] = 3.0;
JacPs[1][0] = 4.0;
JacPs[1][1] = 5.0;
pc.printf("%f ", JacPs[0][0]);
}
void StateMachine()
{
switch (Active_State)
{
case Calibration:
//calibration actions
//pc.printf("Calibration State");
if (Knop1==false)
{
pc.printf("Entering Homing state \n");
Active_State = Homing;
}
filter();
sample();
//EMG_Read();
Encoding();
break;
case Homing:
//Homing actions
//pc.printf("Homing State");
if (Knop2==false)
{
pc.printf("Entering Funtioning State \n");
Active_State = Function;
}
filter();
sample();
//EMG_Read();
Encoding();
break;
case Function:
//pc.printf("Funtioning State");
if (Knop2==false)
{
pc.printf("Re-entering Homing State \n");
Active_State = Homing;
}
else if (Knop1==false)
{
pc.printf("Re-entering Calibration State \n");
Active_State = Calibration;
}
filter();
sample();
//EMG_Read();
Encoding();
velocity1();
velocity2();
motor1();
motor2();
break;
default:
pc.printf("UNKNOWN COMMAND");
}
}
int main()
{
pc.baud(115200);
PwmPin1.period_us(30); //60 microseconds pwm period, 16.7 kHz
bqc1.add( &BqNotch1_1 ).add( &BqNotch2_1 ).add( &BqHP1 );
bqc2.add(&BqLP1);
bqc3.add( &BqNotch1_2 ).add( &BqNotch2_2 ).add( &BqHP2 );
bqc4.add(&BqLP2);
bqc5.add( &BqNotch1_3 ).add( &BqNotch2_3 ).add( &BqHP3 );
bqc6.add(&BqLP3);
bqc7.add( &BqNotch1_4 ).add( &BqNotch2_4 ).add( &BqHP4 );
bqc8.add(&BqLP4);
StateTicker.attach(StateMachine, 0.002);
printTicker.attach(&Printing, 2.0);
while(true)
{
}
}