Joost Herijgers
EMG and motor script together, Not fully working yet,
Dependencies: Encoder QEI biquadFilter mbed
Revision 5:81d3b53087c0, committed 2017-10-27
- Comitter:
- Joost38H
- Date:
- Fri Oct 27 08:44:21 2017 +0000
- Parent:
- 4:fddab1c875a9
- Commit message:
- new mastercode, with code for three motors
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r fddab1c875a9 -r 81d3b53087c0 main.cpp
--- a/main.cpp Thu Oct 26 11:32:53 2017 +0000
+++ b/main.cpp Fri Oct 27 08:44:21 2017 +0000
@@ -3,121 +3,126 @@
#include "encoder.h"
#include "QEI.h"
#include "BiQuad.h"
-
+
Serial pc(USBTX, USBRX);
-
+
//Defining all in- and outputs
//EMG input
AnalogIn emgBR( A0 ); //Right Biceps
AnalogIn emgBL( A1 ); //Left Biceps
-
+
//Output motor 1 and reading Encoder motor 1
DigitalOut motor1DirectionPin(D4);
PwmOut motor1MagnitudePin(D5);
QEI Encoder1(D12,D13,NC,32);
-
-
+
//Output motor 2 and reading Encoder motor 2
DigitalOut motor2DirectionPin(D7);
PwmOut motor2MagnitudePin(D6);
QEI Encoder2(D10,D11,NC,32);
+//Output motor 3 and reading Encoder motor 3
+DigitalOut motor3DirectionPin(D8);
+PwmOut motor3MagnitudePin(D9);
+QEI Encoder3(D2,D3,NC,32);
+
//LED output, needed for feedback
DigitalOut led_R(LED_RED);
DigitalOut led_G(LED_GREEN);
DigitalOut led_B(LED_BLUE);
-
+
//Setting Tickers for sampling EMG and determing if the threshold is met
-Ticker sample_timer;
-Ticker threshold_timerR;
-Ticker threshold_timerL;
-
-Timer t_thresholdR;
-Timer t_thresholdL;
-
-double currentTimeTR;
-double currentTimeTL;
-
+Ticker sample_timer;
+Ticker threshold_timerR;
+Ticker threshold_timerL;
+
+Timer t_thresholdR;
+Timer t_thresholdL;
+
+double currentTimeTR;
+double currentTimeTL;
+
InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG
-
+
Timer t;
double speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo?
-
+
// Getting the counts from the Encoder
int counts1 = Encoder1.getPulses();
int counts2 = Encoder2.getPulses();
-
+int counts3 = Encoder3.getPulses();
+
// Defining variables delta (the difference between position and desired position) <- Is dit zo?
int delta1;
int delta2;
+int delta3;
-bool Move_done = false;
-
+// Boolean needed to know if new input coordinates have to be given
+bool Move_done = false;
+
/* Defining all the different BiQuad filters, which contain a Notch filter,
High-pass filter and Low-pass filter. The Notch filter cancels all frequencies
between 49 and 51 Hz, the High-pass filter cancels all frequencies below 20 Hz
-and the Low-pass filter cancels out all frequencies below 4 Hz */
-
+and the Low-pass filter cancels out all frequencies below 4 Hz. The filters are
+declared four times, so that they can be used for sampling of right and left
+biceps, during measurements and calibration. */
+
/* Defining all the normalized values of b and a in the Notch filter for the
creation of the Notch BiQuad */
-
-BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
-BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
-
-BiQuad bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
-BiQuad bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
-
+
+BiQuad bqNotch1( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+BiQuad bqNotch2( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+
+BiQuad bqNotchTR( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+BiQuad bqNotchTL( 0.9876, -1.5981, 0.9876, -1.5981, 0.9752 );
+
/* Defining all the normalized values of b and a in the High-pass filter for the
creation of the High-pass BiQuad */
-
-BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
-BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
-
-BiQuad bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
-BiQuad bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
-
+
+BiQuad bqHigh1( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+BiQuad bqHigh2( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+
+BiQuad bqHighTR( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+BiQuad bqHighTL( 0.8371, -1.6742, 0.8371, -1.6475, 0.7009 );
+
/* Defining all the normalized values of b and a in the Low-pass filter for the
creation of the Low-pass BiQuad */
-
-BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
-BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
-
-BiQuad bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
-BiQuad bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
-
+
+BiQuad bqLow1( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+BiQuad bqLow2( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+
+BiQuad bqLowTR( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+BiQuad bqLowTL( 6.0985e-4, 0.0012, 6.0985e-4, -1.9289, 0.9314 );
+
// Creating a variable needed for the creation of the BiQuadChain
-BiQuadChain bqChain1;
-BiQuadChain bqChain2;
-
-BiQuadChain bqChainTR;
-BiQuadChain bqChainTL;
-
-//Defining all doubles needed in the filtering process
-double emgBRfiltered; //Right biceps Notch+High pass filter
-double emgBRrectified; //Right biceps rectified
-double emgBRcomplete; //Right biceps low-pass filter, filtering complete
+BiQuadChain bqChain1;
+BiQuadChain bqChain2;
+
+BiQuadChain bqChainTR;
+BiQuadChain bqChainTL;
+
+//Declaring all doubles needed in the filtering process
+double emgBRfiltered; //Right biceps Notch+High pass filter
+double emgBRrectified; //Right biceps rectified
+double emgBRcomplete; //Right biceps low-pass filter, filtering complete
+
+double emgBLfiltered; //Left biceps Notch+High pass filter
+double emgBLrectified; //Left biceps rectified
+double emgBLcomplete; //Left biceps low-pass filter, filtering complete
-double emgBLfiltered; //Left biceps Notch+High pass filter
-double emgBLrectified; //Left biceps rectified
-double emgBLcomplete; //Left biceps low-pass filter, filtering complete
-
-int countBR = 0;
-int countBL = 0;
-
-//double threshold = 0.03;
-
-double numsamples = 500;
-double emgBRsum = 0;
-double emgBRmeanMVC;
-double thresholdBR;
-
-double emgBLsum = 0;
-double emgBLmeanMVC;
-double thresholdBL;
-
-/* Function to sample the EMG and get a Threshold value from it,
-which can be used throughout the process */
-
+// Declaring all variables needed for getting the Threshold value
+double numsamples = 500;
+double emgBRsum = 0;
+double emgBRmeanMVC;
+double thresholdBR;
+
+double emgBLsum = 0;
+double emgBLmeanMVC;
+double thresholdBL;
+
+/* Function to sample the EMG of the Right Biceps and get a Threshold value
+from it, which can be used throughout the process */
+
void Threshold_samplingBR() {
t_thresholdR.start();
currentTimeTR = t_thresholdR.read();
@@ -132,11 +137,12 @@
}
emgBRmeanMVC = emgBRsum/numsamples;
thresholdBR = emgBRmeanMVC * 0.20;
-
-
-
+
//pc.printf("ThresholdBR = %f \n", thresholdBR);
}
+/* Function to sample the EMG of the Left Biceps and get a Threshold value
+from it, which can be used throughout the process */
+
void Threshold_samplingBL() {
t_thresholdL.start();
currentTimeTL = t_thresholdL.read();
@@ -152,64 +158,130 @@
emgBLmeanMVC = emgBLsum/numsamples;
thresholdBL = emgBLmeanMVC * 0.20;
-
+
}
+
+// EMG sampling and filtering
-// EMG functions
void EMG_sample()
{
//Filtering steps for the Right Biceps EMG
emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass
emgBRrectified = fabs(emgBRfiltered); //Rectification
emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass
-
-
- /*Getting threshold value for Right Biceps, a value of 20% of
- Maximum Voluntary Contraction is chosen as threshold value */
- /*if (countBR < numsamples) {
- emgBRsum = emgBRsum + emgBRcomplete;
- countBR++;
- led_R = 0;
- led_B = 0;
- led_G = 1;
- }
-
- emgBRmeanMVC = emgBRsum / numsamples;
-
- thresholdBR = emgBRmeanMVC * 0.25;*/
-
+
//Filtering steps for the Left Biceps EMG
emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
emgBLrectified = fabs( emgBLfiltered ); //Rectification
emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
-
-
- /*Getting threshold value for Left Biceps, a value of 20% of
- Maximum Voluntary Contraction is chosen as threshold value */
- /*if (countBL < numsamples) {
- emgBLsum = emgBLsum + emgBLcomplete;
- countBL++;
- }
-
- emgBLmeanMVC = emgBLsum / numsamples;
-
- thresholdBL = emgBLmeanMVC * 0.25;
-
- //pc.printf("ThresholdBR = %0.3f, ThresholdBL = %0.3f \n", thresholdBR,thresholdBL);*/
+
}
-
-// Function to make the BiQuadChain for the Notch and High pass filter for both filters
+// Function to make the BiQuadChain for the Notch and High pass filter for all three filters
void getbqChain()
{
- bqChain1.add(&bqNotch1).add(&bqHigh1);
+ bqChain1.add(&bqNotch1).add(&bqHigh1); //Making the BiQuadChain
bqChain2.add(&bqNotch2).add(&bqHigh2);
+
+ bqChainTR.add(&bqNotchTR).add(&bqHighTR);
+ bqChainTL.add(&bqNotchTR).add(&bqHighTL);
}
-
-// Initial input value for X
+
+// Initial input value for couting the X-values
int Xin=0;
-int Xin_new; //<- Hoe zit het met deze als we de robot daadwerkelijk gebruiken
+int Xin_new;
double huidigetijdX;
-
+
+// Feedback system for counting values of X
+void ledtX(){
+ t.reset();
+ Xin++;
+ pc.printf("Xin is %i\n",Xin);
+ led_G=0;
+ led_R=1;
+ wait(0.2);
+ led_G=1;
+ led_R=0;
+ wait(0.5);
+}
+
+// Couting system for values of X
+int tellerX(){
+ if (Move_done == true) {
+ t.reset();
+ led_G=1;
+ led_B=1;
+ led_R=0;
+ while(true){
+ //button.fall(ledtX);
+ if (emgBRcomplete > thresholdBR) {
+ ledtX();
+ }
+ t.start();
+ huidigetijdX=t.read();
+ if (huidigetijdX>2){
+ led_R=1; //Go to the next program (counting values for Y)
+ Xin_new = Xin;
+ Xin = 0;
+
+ return Xin_new;
+ }
+
+ }
+
+ }
+ return 0;
+}
+
+// Initial values needed for Y (see comments at X function)
+int Yin=0;
+int Yin_new;
+double huidigetijdY;
+
+//Feedback system for couting values of Y
+void ledtY(){
+ t.reset();
+ Yin++;
+ pc.printf("Yin is %i\n",Yin);
+ led_G=0;
+ led_B=1;
+ wait(0.2);
+ led_G=1;
+ led_B=0;
+ wait(0.5);
+}
+
+// Couting system for values of Y
+int tellerY(){
+ if (Move_done == true) {
+ t.reset();
+ led_G=1;
+ led_B=0;
+ led_R=1;
+ while(true){
+ //button.fall(ledtY);
+ if (emgBRcomplete > thresholdBR) {
+ ledtY();
+ }
+ t.start();
+ huidigetijdY=t.read();
+ if (huidigetijdY>2){
+ led_B=1;
+ Yin_new = Yin;
+ Yin = 0;
+
+ Move_done = false;
+ return Yin_new;
+
+ }
+ }
+ }
+ return 0; // ga door naar het volgende programma
+}
+
+
+
+ // Oude shit voor input waardes geven
+/*---------------------------------------------------------------------------------
// Feedback system for counting values of X
void ledtX(){
t.reset();
@@ -226,37 +298,28 @@
// Couting system for values of X
int tellerX(){
- if (Move_done == true) {
- t.reset();
- led_G=1;
- led_B=1;
- led_R=0;
+ led_G=1;
+ led_B=1;
+ led_R=0;
while(true){
- button.fall(ledtX); //This has to be replaced by EMG
+ //button.fall(ledtX); //This has to be replaced by EMG
+ if (emgBRcomplete > thresholdBR){
+ ledtX(); // dit is wat je uiteindelijk wil dat er staat
+ }
+ t.start();
+ huidigetijdX=t.read();
+ if (huidigetijdX>2){
+ led_R=1; //Go to the next program (couting values for Y)
if (emgBRcomplete > thresholdBR){
- ledtX(); // dit is wat je uiteindelijk wil dat er staat
+ 0; // dit is wat je uiteindelijk wil dat er staat
}
-
- t.start();
- huidigetijdX=t.read();
- if (huidigetijdX>2){
- led_R=1; //Go to the next program (couting values for Y)
- Xin_new = Xin;
- Xin = 0;
- //button.fall(0); // Wat is deze?
-
- return Xin_new;
- }
-
- }
-
- }
- return 0;
+ return 0;
+ }
+ }
}
// Initial values needed for Y (see comments at X function)
int Yin=0;
-int Yin_new;
double huidigetijdY;
//Feedback system for couting values of Y
@@ -274,7 +337,6 @@
// Couting system for values of Y
int tellerY(){
- if (Move_done == true) {
t.reset();
led_G=1;
led_B=0;
@@ -283,136 +345,202 @@
//button.fall(ledtY); //See comments at X
if (emgBRcomplete > thresholdBR){
ledtY(); // dit is wat je uiteindelijk wil dat er staat
- }
+ }
t.start();
huidigetijdY=t.read();
if (huidigetijdY>2){
led_B=1;
- Yin_new = Yin;
- Yin = 0;
- //if (emgBRcomplete > thresholdBR){
- //0; // dit is wat je uiteindelijk wil dat er staat
- //}
+ if (emgBRcomplete > thresholdBR){
+ 0; // dit is wat je uiteindelijk wil dat er staat
+ }
+
//button.fall(0); // Wat is deze?
- Move_done = false;
- return Yin_new;
-
+ return 0; // ga door naar het volgende programma
}
}
- }
- return 0; // ga door naar het volgende programma
}
+*/
-
+
// Declaring all variables needed for calculating rope lengths,
-double Pox = 0;
-double Poy = 0;
-double Pbx = 0;
-double Pby = 887;
-double Prx = 768;
-double Pry = 443;
-double Pex=121;
-double Pey=308;
-double diamtrklosje=20;
-double pi=3.14159265359;
-double omtrekklosje=diamtrklosje*pi;
-double Lou;
-double Lbu;
-double Lru;
-double dLod;
-double dLbd;
-double dLrd;
-
+double Pox = 0;
+double Poy = 0;
+double Pbx = 0;
+double Pby = 887;
+double Prx = 768;
+double Pry = 443;
+double Pex=121;
+double Pey=308;
+double diamtrklosje=20;
+double pi=3.14159265359;
+double omtrekklosje=diamtrklosje*pi;
+double Lou;
+double Lbu;
+double Lru;
+double dLod;
+double dLbd;
+double dLrd;
+
// Declaring variables needed for calculating motor counts
-double roto;
-double rotb;
-double rotr;
-double rotzo;
-double rotzb;
-double rotzr;
-double counto;
-double countb;
-double countr;
-double countzo;
-double countzb;
-double countzr;
-
+double roto;
+double rotb;
+double rotr;
+double rotzo;
+double rotzb;
+double rotzr;
+double counto;
+double countb;
+double countr;
+double countzo;
+double countzb;
+double countzr;
+
+double hcounto;
+double dcounto;
+double hcountb;
+double dcountb;
+double hcountr;
+double dcountr;
+
// Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit?
-double Psx;
-double Psy;
-double Vex;
-double Vey;
-double Kz=0.7; // nadersnelheid instellen
-double modVe;
-double Vmax=20;
-double Pstx;
-double Psty;
-double T=0.02;//seconds
-
-
+double Psx;
+double Psy;
+double Vex;
+double Vey;
+double Kz=0.7; // nadersnelheid instellen
+double modVe;
+double Vmax=20;
+double Pstx;
+double Psty;
+double T=0.02;//seconds
+
+double speedfactor1;
+double speedfactor2;
+double speedfactor3;
+
+
//Deel om motor(en) aan te sturen--------------------------------------------
-
+
+
+
void calcdelta1() {
- delta1 = (counto - Encoder1.getPulses());
+ delta1 = (dcounto - Encoder1.getPulses());
+ }
+
+void calcdelta2() {
+ delta2 = (dcountb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse)
}
+
+void calcdelta3() {
+ delta3 = (dcountr - Encoder3.getPulses()); // <------- de reden dat de delta negatief is (jitse)
+ }
+
+double referenceVelocity1;
+double motorValue1;
+
+double referenceVelocity2;
+double motorValue2;
-void calcdelta2() {
- delta2 = (countb - Encoder2.getPulses()); // <------- de reden dat de delta negatief is (jitse)
- }
-
-double referenceVelocity1;
-double motorValue1;
+double referenceVelocity3;
+double motorValue3;
+
+Ticker controlmotor1; // één ticker van maken?
+Ticker calculatedelta1;
+Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
+
+Ticker controlmotor2; // één ticker van maken?
+Ticker calculatedelta2;
+Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
-double referenceVelocity2;
-double motorValue2;
-
-Ticker controlmotor1; // één ticker van maken?
-Ticker calculatedelta1;
-Ticker printdata1; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
-
-Ticker controlmotor2; // één ticker van maken?
-Ticker calculatedelta2;
-Ticker printdata2; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
-
+Ticker controlmotor3; // één ticker van maken?
+Ticker calculatedelta3;
+Ticker printdata3; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
+
double GetReferenceVelocity1()
{
// Returns reference velocity in rad/s. Positive value means clockwise rotation.
double maxVelocity1=Vex*25+Vey*25; // max 8.4 in rad/s of course!
- referenceVelocity1 = (-1)*speedfactor * maxVelocity1;
+ referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;
- if (Encoder1.getPulses() < (counto+1))
- { speedfactor = 0.1;
+ if (dcounto < (10))
+ { speedfactor1 = 0.01;
+ if (dcounto > (-10))
+ { printf("kleiner111111111");
+ speedfactor1=0;
+ }
}
- else if (Encoder1.getPulses() > (counto-1))
- { speedfactor = -0.1;
+ else if (dcounto > (-10))
+ { speedfactor1 = -0.01;
+ if (dcounto < (10))
+ { printf("groter");
+ speedfactor1=0;
+ }
}
- else
- { speedfactor = 0;
+ else
+ { speedfactor1 = 0;
+ pc.printf("speedfactor nul;");
}
return referenceVelocity1;
}
-
+
double GetReferenceVelocity2()
{
// Returns reference velocity in rad/s. Positive value means clockwise rotation.
double maxVelocity2=Vex*25+Vey*25; // max 8.4 in rad/s of course!
- referenceVelocity2 = (-1)*speedfactor * maxVelocity2;
+ referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;
- if (Encoder2.getPulses() < (counto+1))
- { speedfactor = 0.1;
+ if (Encoder2.getPulses() < (dcountb+10))
+ { speedfactor2 = -0.01;
+ if (Encoder2.getPulses() > (dcountb-10))
+ { //printf("kleiner22222222222");
+ speedfactor2=0;
+ }
}
- else if (Encoder2.getPulses() > (counto-1))
- { speedfactor = -0.1;
+ else if (Encoder2.getPulses() > (dcountb-10))
+ { speedfactor2 = 0.01;
+ if (Encoder2.getPulses() < (dcountb+10))
+ { //printf("groter");
+ speedfactor2=0;
+ }
}
- else
- { speedfactor = 0;
+ else
+ { speedfactor2 = 0;
+ //pc.printf("speedfactor nul;");
}
return referenceVelocity2;
-}
+}
+double GetReferenceVelocity3()
+{
+ // Returns reference velocity in rad/s. Positive value means clockwise rotation.
+ double maxVelocity3=Vex*25+Vey*25; // max 8.4 in rad/s of course!
+ referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;
+
+ if (Encoder3.getPulses() < (dcountr+10))
+ { speedfactor3 = -0.01;
+ if (Encoder3.getPulses() > (dcountr-10))
+ { //printf("kleiner22222222222");
+ speedfactor3=0;
+ }
+ }
+ else if (Encoder3.getPulses() > (dcountr-10))
+ { speedfactor3 = 0.01;
+ if (Encoder3.getPulses() < (dcountr+10))
+ { //printf("groter");
+ speedfactor3=0;
+ }
+ }
+ else
+ { speedfactor3 = 0;
+ //pc.printf("speedfactor nul;");
+ }
+
+ return referenceVelocity3;
+}
+
void SetMotor1(double motorValue1)
{
// Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
@@ -423,7 +551,7 @@
else motor1MagnitudePin = fabs(motorValue1);
}
-
+
void SetMotor2(double motorValue2)
{
// Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
@@ -435,6 +563,17 @@
}
+void SetMotor3(double motorValue3)
+{
+ // Given -1<=motorValue<=1, this sets the PWM and direction bits for motor 1. Positive value makes
+ // motor rotating clockwise. motorValues outside range are truncated to within range
+ if (motorValue3 >=0) motor3DirectionPin=1;
+ else motor3DirectionPin=0;
+ if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
+ else motor3MagnitudePin = fabs(motorValue3);
+
+}
+
double FeedForwardControl1(double referenceVelocity1)
{
// very simple linear feed-forward control
@@ -442,7 +581,7 @@
double motorValue1 = referenceVelocity1 / MotorGain;
return motorValue1;
}
-
+
double FeedForwardControl2(double referenceVelocity2)
{
// very simple linear feed-forward control
@@ -450,6 +589,14 @@
double motorValue2 = referenceVelocity2 / MotorGain;
return motorValue2;
}
+
+double FeedForwardControl3(double referenceVelocity3)
+{
+ // very simple linear feed-forward control
+ const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
+ double motorValue3 = referenceVelocity3 / MotorGain;
+ return motorValue3;
+}
void MeasureAndControl1()
{
@@ -459,7 +606,7 @@
double motorValue1 = FeedForwardControl1(referenceVelocity1);
SetMotor1(motorValue1);
}
-
+
void MeasureAndControl2()
{
// This function measures the potmeter position, extracts a reference velocity from it,
@@ -469,24 +616,40 @@
SetMotor2(motorValue2);
}
+void MeasureAndControl3()
+{
+ // This function measures the potmeter position, extracts a reference velocity from it,
+ // and controls the motor with a simple FeedForward controller. Call this from a Ticker.
+ double referenceVelocity3 = GetReferenceVelocity3();
+ double motorValue3 = FeedForwardControl3(referenceVelocity3);
+ SetMotor3(motorValue3);
+}
+
void readdata1()
{ //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
- pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
+ //pc.printf("Pulses_M1 = %i \r\n",Encoder1.getPulses());
//pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
- pc.printf("Delta_M1 = %i \r\n",delta1);
+ //pc.printf("Delta_M1 = %i \r\n",delta1);
}
void readdata2()
{ //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
- pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
+ //pc.printf("Pulses_M2 = %i \r\n",Encoder2.getPulses());
//pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
- pc.printf("Delta_M2 = %i \r\n",delta2);
+ //pc.printf("Delta_M2 = %i \r\n",delta2);
}
-
+
+void readdata3()
+ { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
+ //pc.printf("Pulses_M2 = %i \r\n",Encoder3.getPulses());
+ //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
+ //pc.printf("Delta_M2 = %i \r\n",delta3);
+ }
+
// einde deel motor------------------------------------------------------------------------------------
-
+
Ticker loop;
-
+
/*Calculates ropelengths that are needed to get to new positions, based on the
set coordinates and the position of the poles */
double touwlengtes(){
@@ -495,7 +658,7 @@
Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
return 0;
}
-
+
/* Calculates rotations (and associated counts) of the motor to get to the
desired new position*/
double turns(){
@@ -504,14 +667,17 @@
rotb=Lbu/omtrekklosje;
rotr=Lru/omtrekklosje;
counto=roto*4200;
- //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts
+ dcounto=counto-hcounto;
+ pc.printf("dcounto = %f \n\r",dcounto);
countb=rotb*4200;
- //countb = (int)(countb + 0.5);
+ dcountb=countb-hcountb;
+ pc.printf("dcountb = %f \n\r",dcountb);
countr=rotr*4200;
- //countr = (int)(countr + 0.5);
+ dcountr=countr-hcountr;
+
return 0;
}
-
+
// Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
double Pst(){
Pstx=Pex+Vex*T;
@@ -519,7 +685,7 @@
touwlengtes();
Pex=Pstx;
Pey=Psty;
- //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
+ pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
//pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
turns();
//pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr);
@@ -529,15 +695,19 @@
pc.printf("\n\r R=%f",R);*/
return 0;
}
-
+
//Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
double Ps(){
+ if (Move_done==true);
Psx=(Xin_new)*30+121;
Psy=(Yin_new)*30+308;
- //pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
+ pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
+ hcounto=4200*((sqrt(pow((Pex-Pox),2)+pow((Pey-Poy),2)))/omtrekklosje);
+ hcountb=4200*((sqrt(pow((Pex-Pbx),2)+pow((Pey-Pby),2)))/omtrekklosje);
+ hcountr=4200*((sqrt(pow((Pex-Prx),2)+pow((Pey-Pry),2)))/omtrekklosje);
return 0;
}
-
+
// Rekent dit de snelheid uit waarmee de motoren bewegen?
void Ve(){
Vex=Kz*(Psx-Pex);
@@ -548,20 +718,20 @@
Vey=(Vey/modVe)*Vmax;
}
Pst();
- pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
+ //pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
Move_done=true;
loop.detach();
}
}
-
+
// Calculating the desired position, so that the motors can go here
int calculator(){
Ps();
loop.attach(&Ve,0.02);
return 0;
}
-
+
// Function which makes it possible to lower the end-effector to pick up a piece
void zakker(){
while(1){
@@ -577,11 +747,11 @@
countzb=rotzb*4200;
countzr=rotzr*4200;
- pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
+ //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
}
}
}
-
+
int main()
{
pc.baud(115200);
@@ -591,16 +761,22 @@
while(true){
sample_timer.attach(&EMG_sample, 0.002);
wait(2.5f);
+
tellerX();
tellerY();
calculator();
controlmotor1.attach(&MeasureAndControl1, 0.01);
calculatedelta1.attach(&calcdelta1, 0.01);
- //printdata1.attach(&readdata1, 0.5);
- controlmotor2.attach(&MeasureAndControl2, 0.01);
- calculatedelta2.attach(&calcdelta2, 0.01);
+ printdata1.attach(&readdata1, 0.5);
+ //controlmotor2.attach(&MeasureAndControl2, 0.01);
+ //calculatedelta2.attach(&calcdelta2, 0.01);
//printdata2.attach(&readdata2, 0.5);
+ //controlmotor3.attach(&MeasureAndControl3, 0.01);
+ //calculatedelta3.attach(&calcdelta3, 0.01);
+ //printdata3.attach(&readdata3, 0.5);
//zakker();
+ wait(5.0f);
}
- return 0;
-}
\ No newline at end of file
+
+}
+
\ No newline at end of file