Joost Herijgers
EMG and motor script together, Not fully working yet,.
Dependencies: Encoder MODSERIAL QEI biquadFilter mbed
Fork of
Code_MotorEMG
by 
Joost Herijgers
Diff: main.cpp
- Revision:
- 10:952377fbbbfe
- Parent:
- 9:3874b23bb233
- Child:
- 11:f23fe69ba3ef
--- a/main.cpp Tue Oct 31 16:24:35 2017 +0000
+++ b/main.cpp Wed Nov 01 11:18:18 2017 +0000
@@ -1,6 +1,6 @@
#include "mbed.h"
#include "math.h"
-#include "encoder.h"
+//#include "encoder.h"
#include "QEI.h"
#include "BiQuad.h"
@@ -39,18 +39,13 @@
Timer t_thresholdR;
Timer t_thresholdL;
-double currentTimeTR;
-double currentTimeTL;
+float currentTimeTR;
+float 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();
+float speedfactor; // = 0.01; snelheid in, zonder potmeter gebruik <- waarom is dit zo?
// Defining variables delta (the difference between position and desired position) <- Is dit zo?
int delta1;
@@ -101,24 +96,24 @@
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
+//Declaring all floats needed in the filtering process
+float emgBRfiltered; //Right biceps Notch+High pass filter
+float emgBRrectified; //Right biceps rectified
+float 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
+float emgBLfiltered; //Left biceps Notch+High pass filter
+float emgBLrectified; //Left biceps rectified
+float emgBLcomplete; //Left biceps low-pass filter, filtering complete
// Declaring all variables needed for getting the Threshold value
-double numsamples = 500;
-double emgBRsum = 0;
-double emgBRmeanMVC;
-double thresholdBR;
+float numsamples = 500;
+float emgBRsum = 0;
+float emgBRmeanMVC;
+float thresholdBR;
-double emgBLsum = 0;
-double emgBLmeanMVC;
-double thresholdBL;
+float emgBLsum = 0;
+float emgBLmeanMVC;
+float thresholdBL;
/* Function to sample the EMG of the Right Biceps and get a Threshold value
from it, which can be used throughout the process */
@@ -189,7 +184,7 @@
// Initial input value for couting the X-values
int Xin=0;
int Xin_new;
-double huidigetijdX;
+float huidigetijdX;
// Feedback system for counting values of X
void ledtX(){
@@ -235,7 +230,7 @@
// Initial values needed for Y (see comments at X function)
int Yin=0;
int Yin_new;
-double huidigetijdY;
+float huidigetijdY;
//Feedback system for couting values of Y
void ledtY(){
@@ -278,286 +273,134 @@
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();
- 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(){
- led_G=1;
- led_B=1;
- led_R=0;
- while(true){
- //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){
- 0; // dit is wat je uiteindelijk wil dat er staat
- }
- return 0;
- }
- }
-}
-
-// Initial values needed for Y (see comments at X function)
-int Yin=0;
-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(){
- t.reset();
- led_G=1;
- led_B=0;
- led_R=1;
- while(true){
- //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;
- if (emgBRcomplete > thresholdBR){
- 0; // dit is wat je uiteindelijk wil dat er staat
- }
-
- //button.fall(0); // Wat is deze?
- 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=91;
-double Pey=278;
-double diamtrklosje=20;
-double pi=3.14159265359;
-double omtrekklosje=diamtrklosje*pi;
-double Lou;
-double Lbu;
-double Lru;
-double dLod;
-double dLbd;
-double dLrd;
+float Pox = 0;
+float Poy = 0;
+float Pbx = 0;
+float Pby = 887;
+float Prx = 768;
+float Pry = 443;
+float Pex=91;
+float Pey=278;
+float diamtrklosje=20;
+float pi=3.14159265359;
+float omtrekklosje=diamtrklosje*pi;
+float Lou;
+float Lbu;
+float Lru;
+float dLod;
+float dLbd;
+float 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 erroro;
-double errorb;
-double errorr;
+float roto;
+float rotb;
+float rotr;
+float rotzo;
+float rotzb;
+float rotzr;
+float counto;
+float countb;
+float countr;
+float countzo;
+float countzb;
+float countzr;
+float erroro;
+float errorb;
+float errorr;
-double hcounto;
-double dcounto;
-double hcountb;
-double dcountb;
-double hcountr;
-double dcountr;
+float hcounto;
+float dcounto;
+float hcountb;
+float dcountb;
+float hcountr;
+float 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
+float Psx;
+float Psy;
+float Vex;
+float Vey;
+float Kz=0.7; // nadersnelheid instellen
+float modVe;
+float Vmax=20;
+float Pstx;
+float Psty;
+float T=0.02;//seconds
-double kpo = 4;
-double kpb = 4;
-double kpr = 4;
+float kpo = 0.1;
+float kpb = 0.1;
+float kpr = 0.1;
-double speedfactor1;
-double speedfactor2;
-double speedfactor3;
+float speedfactor1;
+float speedfactor2;
+float speedfactor3;
-//Deel om motor(en) aan te sturen--------------------------------------------
-
-
-
-void calcdelta1() {
- delta1 = (dcounto - Encoder1.getPulses());
- }
+//Deel om motor(en) aan te sturen--------------------------------------------
+float referenceVelocity1;
+float motorValue1;
-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;
+float referenceVelocity2;
+float motorValue2;
-double referenceVelocity3;
-double motorValue3;
+float referenceVelocity3;
+float 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
-
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()
-{
- //Berekenen error O, d.m.v. je gewenste positie (in kleine stapjes), je beginpositie (allereerste keer dus 1,1) en de counts van Encoder 1
- erroro=counto-hcounto-Encoder1.getPulses();
- pc.printf("erroro = %.2f \n\r", erroro);
- // Returns reference velocity in rad/s. Positive value means clockwise rotation.
- double maxVelocity1=fabs(Vex+Vey); // max 8.4 in rad/s of course!
- referenceVelocity1 = (-1)*speedfactor1 * maxVelocity1;
-
- if (Encoder1.getPulses() < (dcounto+10))
- { speedfactor1 = 0.05;
- //pc.printf("Zit je hier?");
- if (Encoder1.getPulses() > (dcounto-10))
- { //pc.printf("kleiner");
- speedfactor1=0;
- }
- }
- else if ((Encoder1.getPulses()) > (dcounto-10))
- { speedfactor1 = -0.05;
- if (Encoder1.getPulses() < (dcounto+10))
- { //pc.printf("groter");
- speedfactor1=0;
- }
- }
- else
- { speedfactor1 = 0;
- pc.printf("speedfactor nul;");
- }
-
- return referenceVelocity1;
- return kpo*erroro;
-} */
-double P1(double erroro, double kpo) {
+float P1(float erroro, float kpo) {
return erroro*kpo;
}
void MotorController1()
{
- double reference_o = counto-hcounto;
- double position_o = Encoder1.getPulses();
+ float reference_o = counto-hcounto;
+ int position_o = Encoder1.getPulses();
+
+ erroro = reference_o - position_o;
+
+ pc.printf("Position_o = %i \n\r",position_o);
- motorValue1 = P1(reference_o - position_o, kpo);
+ if (-100<erroro && erroro<100){
+ motorValue1 = 0;
+ }
+ else {
+ motorValue1 = 0.1*P1(erroro, kpo);
+ }
+
if (motorValue1 >=0) motor1DirectionPin=0;
else motor1DirectionPin=1;
if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
else motor1MagnitudePin = fabs(motorValue1);
}
-/*
-double GetReferenceVelocity2()
-{
- errorb=countb-hcountb-Encoder2.getPulses();
- pc.printf("erroro = %.2f \n\r", errorb);
- // Returns reference velocity in rad/s. Positive value means clockwise rotation.
- double maxVelocity2=fabs(Vex+Vey); // max 8.4 in rad/s of course!
- referenceVelocity2 = (-1)*speedfactor2 * maxVelocity2;
-
- if (Encoder2.getPulses() < (dcountb+10))
- { speedfactor2 = 0.05;
- if (Encoder2.getPulses() > (dcountb-10))
- { //printf("kleiner22222222222");
- speedfactor2=0;
- }
- }
- else if (Encoder2.getPulses() > (dcountb-10))
- { speedfactor2 = -0.05;
- if (Encoder2.getPulses() < (dcountb+10))
- { //printf("groter");
- speedfactor2=0;
- }
- }
- else
- { speedfactor2 = 0;
- //pc.printf("speedfactor nul;");
- }
-
- return referenceVelocity2;
-
- return kpb*errorb;
-} */
+
-double P2(double errorb, double kpb) {
+float P2(float errorb, float kpb) {
return errorb*kpb;
}
void MotorController2()
{
- double reference_b = countb-hcountb;
- double position_b = Encoder2.getPulses();
+ float reference_b = countb-hcountb;
+ int position_b = Encoder2.getPulses();
- motorValue2 = P2(reference_b - position_b, kpb);
+ errorb = reference_b - position_b;
+
+ pc.printf("position_b= %i", position_b);
+
+ if (-100<errorb && errorb<100){
+ motorValue2 = 0;
+ }
+ else {
+ motorValue2 = 0.1*P2(errorb, kpb);
+ }
+
if (motorValue2 >=0) motor2DirectionPin=1;
else motor2DirectionPin=0;
@@ -565,172 +408,45 @@
else motor2MagnitudePin = fabs(motorValue2);
}
-/*
-double GetReferenceVelocity3()
-{
- errorr=countr-hcountr-Encoder3.getPulses();
- pc.printf("erroro = %.2f \n\r", errorb);
-
- // Returns reference velocity in rad/s. Positive value means clockwise rotation.
- double maxVelocity3=fabs(Vex+Vey); // max 8.4 in rad/s of course!
- referenceVelocity3 = (-1)*speedfactor3 * maxVelocity3;
-
- int Counts3 = Encoder3.getPulses();
-
- if (Encoder3.getPulses() < (dcountr+10))
- { speedfactor3 = 0.05;
- if (Encoder3.getPulses() > (dcountr-10))
- { //printf("Encoder waarde %i\n\r", Counts3);
- speedfactor3=0;
- }
- }
- else if (Encoder3.getPulses() > (dcountr-10))
- { speedfactor3 = -0.05;
- if (Encoder3.getPulses() < (dcountr+10))
- { //printf("groter");
- speedfactor3=0;
- }
- }
- else
- { speedfactor3 = 0;
- //pc.printf("speedfactor nul;");
- }
-
- return referenceVelocity3;
- return kpr*errorr;
-} */
+
-double P3(double errorr, double kpr) {
+float P3(float errorr, float kpr) {
return errorr*kpr;
}
void MotorController3()
{
- double reference_r = countr-hcountr;
- double position_r = Encoder3.getPulses();
- pc.printf("reference_r = %0.2f", reference_r);
- pc.printf("position_r = %0.2f", position_r);
+ float reference_r = countr-hcountr;
+ int position_r = Encoder3.getPulses();
+
+ errorr = reference_r - position_r;
+
+ pc.printf("position_r= %i", position_r);
+
- motorValue3 = P3(reference_r - position_r, kpr);
+ if (-100<errorr && errorr<100){
+ motorValue3 = 0;
+
+ }
+ else {
+ motorValue3 = 0.1*P3(errorr, kpr);
+ }
+
if (motorValue3 >=0) motor3DirectionPin=1;
else motor3DirectionPin=0;
if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
else motor3MagnitudePin = fabs(motorValue3);
}
-/*
-
-void SetMotor1()
-{
- // 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 (motorValue1 >=0) motor1DirectionPin=1;
- else motor1DirectionPin=0;
- if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
- else motor1MagnitudePin = fabs(motorValue1);
-
-}
-
-void SetMotor2()
-{
- // 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 (motorValue2 >=0) motor2DirectionPin=1;
- else motor2DirectionPin=0;
- if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
- else motor2MagnitudePin = fabs(motorValue2);
-
-}
-
-void SetMotor3()
-{
- // 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
- const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
- double motorValue1 = referenceVelocity1 / MotorGain;
- return motorValue1;
-}
-
-double FeedForwardControl2(double referenceVelocity2)
-{
- // very simple linear feed-forward control
- const double MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
- 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()
-{
- // 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 referenceVelocity1 = GetReferenceVelocity1();
- motorValue1 = MotorValue1();
- SetMotor1();
-}
-
-void MeasureAndControl2()
-{
- // 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 referenceVelocity2 = GetReferenceVelocity2();
- motorValue2 = MotorValue2();
- SetMotor2();
-}
-void MeasureAndControl3()
-{
- // This function measures the potmeter position, extracts a reference velocity from it,
- // and controls the motor with a simple FeedForwardouble referenceVelocity3 = GetReferenceVelocity3();
- motorValue3 = MotorValue3();
- SetMotor3();
-}*/
-
-void readdata1()
- { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
- //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);
- }
-
-void readdata2()
- { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
- //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);
- }
-
-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(){
+float touwlengtes(){
Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
Lbu=sqrt(pow((Pstx-Pbx),2)+pow((Psty-Pby),2));
Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
@@ -739,7 +455,7 @@
/* Calculates rotations (and associated counts) of the motor to get to the
desired new position*/
-double turns(){
+float turns(){
roto=Lou/omtrekklosje;
rotb=Lbu/omtrekklosje;
@@ -759,17 +475,17 @@
}
// Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
-double Pst(){
+float Pst(){
Pstx=Pex+Vex*T;
Psty=Pey+Vey*T;
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);
- //pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
+ pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
/*float R;
R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constant is en de end efector dus een rechte lijn maakt
pc.printf("\n\r R=%f",R);*/
@@ -777,8 +493,7 @@
}
//Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
-double Ps(){
- if (Move_done==true);
+float Ps(){
Psx=(Xin_new)*30+91;
Psy=(Yin_new)*30+278;
pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
@@ -787,13 +502,13 @@
// Rekent dit de snelheid uit waarmee de motoren bewegen?
void Ve(){
- Vex=Kz*(Psx-Pex);
- Vey=Kz*(Psy-Pey);
- modVe=sqrt(pow(Vex,2)+pow(Vey,2));
+ Vex=0.2*(Psx-Pex);
+ Vey=0.2*(Psy-Pey);
+/* modVe=sqrt(pow(Vex,2)+pow(Vey,2));
if(modVe>Vmax){
Vex=(Vex/modVe)*Vmax;
Vey=(Vey/modVe)*Vmax;
- }
+ }*/
Pst();
pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)){
@@ -805,7 +520,9 @@
// Calculating the desired position, so that the motors can go here
int calculator(){
Ps();
+ if (Move_done == false) {
loop.attach(&Ve,0.02);
+ }
return 0;
}
@@ -837,14 +554,8 @@
Vex = 20;
Vey = 20;
controlmotor1.attach(&MotorController1, 0.01);
- calculatedelta1.attach(&calcdelta1, 0.01);
- printdata1.attach(&readdata1, 0.5);
controlmotor2.attach(&MotorController2, 0.01);
- //calculatedelta2.attach(&calcdelta2, 0.01);
- //printdata2.attach(&readdata2, 0.5);
controlmotor3.attach(&MotorController3, 0.01);
- //calculatedelta3.attach(&calcdelta3, 0.01);
- //printdata3.attach(&readdata3, 0.5);
}
@@ -857,8 +568,6 @@
int main()
{
-
-
pc.baud(115200);
wait(1.0f);
//tiller();
@@ -874,14 +583,8 @@
setcurrentposition();
calculator();
controlmotor1.attach(&MotorController1, 0.01);
- calculatedelta1.attach(&calcdelta1, 0.01);
- printdata1.attach(&readdata1, 0.5);
controlmotor2.attach(&MotorController2, 0.01);
- //calculatedelta2.attach(&calcdelta2, 0.01);
- //printdata2.attach(&readdata2, 0.5);
controlmotor3.attach(&MotorController3, 0.01);
- //calculatedelta3.attach(&calcdelta3, 0.01);
- //printdata3.attach(&readdata3, 0.5);
//zakker();
wait(5.0f);
}