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:
- 21:3fdd135a3dfd
- Parent:
- 20:bde79d7a4091
- Child:
- 22:cdaa5c1208a4
--- a/main.cpp Wed Nov 01 14:36:20 2017 +0000
+++ b/main.cpp Wed Nov 01 16:21:57 2017 +0000
@@ -4,19 +4,19 @@
#include "QEI.h"
#include "BiQuad.h"
#include "MODSERIAL.h"
-
+
MODSERIAL 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);
@@ -26,138 +26,141 @@
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;
-
+
float currentTimeTR;
float currentTimeTL;
-
+
InterruptIn button(SW2); // Wordt uiteindelijk vervangen door EMG
-
+
Timer t;
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;
-int delta2;
+int delta2;
int delta3;
-// Boolean needed to know if new input coordinates have to be given
+// Boolean needed to know if new input coordinates have to be given
bool Move_done = false;
bool Input_done = true;
-
+bool Tiller= 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. 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 );
-
+
/* 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 );
-
+
/* 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 );
-
+
// Creating a variable needed for the creation of the BiQuadChain
BiQuadChain bqChain1;
BiQuadChain bqChain2;
-
+
BiQuadChain bqChainTR;
BiQuadChain bqChainTL;
-
+
//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
-
+
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
+// Declaring all variables needed for getting the Threshold value
float numsamples = 500;
float emgBRsum = 0;
float emgBRmeanMVC;
float thresholdBR;
-
+
float emgBLsum = 0;
float emgBLmeanMVC;
float thresholdBL;
-
-/* Function to sample the EMG of the Right Biceps and get a Threshold value
+
+/* 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() {
+
+void Threshold_samplingBR()
+{
t_thresholdR.start();
currentTimeTR = t_thresholdR.read();
-
+
if (currentTimeTR <= 1) {
-
+
emgBRfiltered = bqChainTR.step( emgBR.read() ); //Notch+High-pass
emgBRrectified = fabs(emgBRfiltered); //Rectification
emgBRcomplete = bqLowTR.step(emgBRrectified); //Low-pass
-
+
emgBRsum = emgBRsum + emgBRcomplete;
- }
- emgBRmeanMVC = emgBRsum/numsamples;
+ }
+ 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
+/* 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();
+void Threshold_samplingBL()
+{
+ t_thresholdL.start();
currentTimeTL = t_thresholdL.read();
-
+
if (currentTimeTL <= 1) {
-
+
emgBLfiltered = bqChain2.step( emgBL.read() ); //Notch+High-pass
emgBLrectified = fabs( emgBLfiltered ); //Rectification
emgBLcomplete = bqLow2.step( emgBLrectified ); //Low-pass
-
+
emgBLsum = emgBLsum + emgBLcomplete;
- }
-
+ }
+
emgBLmeanMVC = emgBLsum/numsamples;
thresholdBL = emgBLmeanMVC * 0.20;
-
+
}
-
+
// EMG sampling and filtering
void EMG_sample()
@@ -166,30 +169,31 @@
emgBRfiltered = bqChain1.step( emgBR.read() ); //Notch+High-pass
emgBRrectified = fabs(emgBRfiltered); //Rectification
emgBRcomplete = bqLow1.step(emgBRrectified); //Low-pass
-
+
//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
-
+
}
-// Function to make the BiQuadChain for the Notch and High pass filter for all three filters
+// Function to make the BiQuadChain for the Notch and High pass filter for all three filters
void getbqChain()
{
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 couting the X-values
-int Xin=0;
-int Xin_new;
+
+// Initial input value for couting the X-values
+int Xin=0;
+int Xin_new;
float huidigetijdX;
-
+
// Feedback system for counting values of X
-void ledtX(){
+void ledtX()
+{
t.reset();
Xin++;
pc.printf("Xin is %i\n",Xin);
@@ -199,43 +203,45 @@
led_G=1;
led_R=0;
wait(0.5);
-}
-
+}
+
// Couting system for values of X
-int tellerX(){
+int tellerX()
+{
if (Move_done == true) {
t.reset();
- led_G=1;
+ 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;
+ 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)
+
+ }
+
+ }
+ return 0;
+}
+
+// Initial values needed for Y (see comments at X function)
int Yin=0;
int Yin_new;
float huidigetijdY;
-
+
//Feedback system for couting values of Y
-void ledtY(){
+void ledtY()
+{
t.reset();
Yin++;
pc.printf("Yin is %i\n",Yin);
@@ -245,37 +251,39 @@
led_G=1;
led_B=0;
wait(0.5);
-}
-
+}
+
// Couting system for values of Y
-int tellerY(){
+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;
- Input_done = true;
- Move_done = false;
- return Yin_new;
-
+ 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;
+ Input_done = true;
+ Move_done = false;
+ Tiller= true;
+ 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,
+
+// Declaring all variables needed for calculating rope lengths,
float Pox = 0;
float Poy = 0;
float Pbx = 0;
@@ -293,7 +301,7 @@
float dLod;
float dLbd;
float dLrd;
-
+
// Declaring variables needed for calculating motor counts
float roto;
float rotb;
@@ -314,7 +322,7 @@
float dcountb;
float hcountr;
float dcountr;
-
+
// Declaring variables neeeded for calculating motor movements to get to a certain point <- klopt dit?
float Psx;
float Psy;
@@ -330,140 +338,267 @@
float kpo = 0.1;
float kpb = 0.1;
float kpr = 0.1;
-
+
float speedfactor1;
float speedfactor2;
float speedfactor3;
-
-
-//Deel om motor(en) aan te sturen--------------------------------------------
-float referenceVelocity1;
-float motorValue1;
-
-float referenceVelocity2;
-float motorValue2;
-
-float referenceVelocity3;
-float motorValue3;
-
+//Deel om motor(en) aan te sturen--------------------------------------------
+float referenceVelocity1;
+float motorValue1;
+
+float referenceVelocity2;
+float motorValue2;
+
+float referenceVelocity3;
+float motorValue3;
+
+
+
Ticker controlmotor1; // één ticker van maken?
Ticker controlmotor2; // één ticker van maken?
Ticker controlmotor3; // één ticker van maken?
-float P1(int erroro, float kpo) {
- return erroro*kpo;
- }
+float P1(int erroro, float kpo)
+{
+ return erroro*kpo;
+}
void MotorController1()
{
- int reference_o = -11987; //(int) (counto-hcounto);
+ int reference_o = (int) (counto-hcounto);
int position_o = Encoder1.getPulses();
-
+
int error_o = reference_o - position_o;
-
+
//pc.printf("Position_o = %i reference_o=%i Error_o=%i\n\r" ,position_o,reference_o,error_o);
-
- if (-100<error_o && error_o<100){
+
+ if (abs(error_o)<100) {
motorValue1 = 0;
- }
- else {
- motorValue1 = 0.05*P1(error_o, kpo);
- }
-
-
-
- if (motorValue1 >=0) motor1DirectionPin=0;
- else motor1DirectionPin=1;
- if (fabs(motorValue1)>1) motor1MagnitudePin = 1;
- else motor1MagnitudePin = fabs(motorValue1);
+ } else {
+ motorValue1 = 0.05*P1(error_o, kpo);
}
-
+
-float P2(int error_b, float kpb) {
- return error_b*kpb;
- }
+ if (motorValue1 >=0) {
+ motor1DirectionPin=0;
+ } else {
+ motor1DirectionPin=1;
+ }
+
+ motor1MagnitudePin = fabs(motorValue1);
+
+
+ if (fabs(motorValue1)>1) {
+ motor1MagnitudePin = 1;
+ }
+}
+
+
+
+float P2(int error_b, float kpb)
+{
+ return error_b*kpb;
+}
void MotorController2()
{
-
- int reference_b = 7648;//(int) (countb-hcountb);
+
+ int reference_b = (int) (-(countb-hcountb));
int position_b = Encoder2.getPulses();
int error_b = reference_b - position_b;
-
+
//pc.printf("Position_b = %i reference_b=%i Error_b=%i " ,position_b,reference_b,error_b);
-
- if (-100<error_b && error_b<100){
+
+ if (-100<error_b && error_b<100) {
motorValue2 = 0;
- }
- else {
- motorValue2 = 0.05*P2(error_b, kpb);
- }
-
-
- if (motorValue2 >=0) motor2DirectionPin=1;
- else motor2DirectionPin=0;
- if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
- else motor2MagnitudePin = fabs(motorValue2);
+ } else {
+ motorValue2 = 0.05*P2(error_b, kpb);
}
-
-float P3(int error_r, float kpr) {
- return error_r*kpr;
- }
+ if (motorValue2 <=0) motor2DirectionPin=0;
+ else motor2DirectionPin=1;
+ if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
+ else motor2MagnitudePin = fabs(motorValue2);
+}
+
+
+
+float P3(int error_r, float kpr)
+{
+ return error_r*kpr;
+}
void MotorController3()
{
- int reference_r = 6886;//(int) (countr-hcountr);
+ int reference_r = (int) (-(countr-hcountr));
int position_r = Encoder3.getPulses();
-
+
int error_r = reference_r - position_r;
-
+
pc.printf("Position_r = %i reference_r=%i Error_r=%i\n\r" ,position_r,reference_r,error_r);
-
-
- if (-100<error_r && error_r<100){
+
+
+ if (-100<error_r && error_r<100) {
motorValue3 = 0;
-
- }
- else {
- motorValue3 = 0.05*P3(error_r, kpr);
- }
-
- if (motorValue3 >=0) motor3DirectionPin=1;
- else motor3DirectionPin=0;
+
+ } else {
+ motorValue3 = 0.05*P3(error_r, kpr);
+ }
+
+ if (motorValue3 <=0) motor3DirectionPin=0;
+ else motor3DirectionPin=1;
if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
- else motor3MagnitudePin = fabs(motorValue3);
- }
-
+ else motor3MagnitudePin = fabs(motorValue3);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void MotorController1t()
+{
+ int reference_o = -9087;
+ int position_o = Encoder1.getPulses();
+
+ int error_o = reference_o - position_o;
+
+ //pc.printf("Position_o = %i reference_o=%i Error_o=%i\n\r" ,position_o,reference_o,error_o);
+
+ if (abs(error_o)<100) {
+ motorValue1 = 0;
+ } else {
+ motorValue1 = 0.05*P1(error_o, kpo);
+ }
+
+ if (motorValue1 >=0) {
+ motor1DirectionPin=0;
+ } else {
+ motor1DirectionPin=1;
+ }
+
+ motor1MagnitudePin = fabs(motorValue1);
+
+
+ if (fabs(motorValue1)>1) {
+ motor1MagnitudePin = 1;
+ }
+}
+
+void MotorController2t()
+{
+
+ int reference_b = 7148;
+ int position_b = Encoder2.getPulses();
+
+ int error_b = reference_b - position_b;
+
+ //pc.printf("Position_b = %i reference_b=%i Error_b=%i " ,position_b,reference_b,error_b);
+
+ if (-100<error_b && error_b<100) {
+ motorValue2 = 0;
+ } else {
+ motorValue2 = 0.05*P2(error_b, kpb);
+ }
+
+
+ if (motorValue2 <=0) motor2DirectionPin=0;
+ else motor2DirectionPin=1;
+ if (fabs(motorValue2)>1) motor2MagnitudePin = 1;
+ else motor2MagnitudePin = fabs(motorValue2);
+}
+
+
+void MotorController3t()
+{
+ int reference_r = 6386;
+ int position_r = Encoder3.getPulses();
+
+ int error_r = reference_r - position_r;
+
+ pc.printf("Position_r = %i reference_r=%i Error_r=%i\n\r" ,position_r,reference_r,error_r);
+
+
+ if (-100<error_r && error_r<100) {
+ motorValue3 = 0;
+
+ } else {
+ motorValue3 = 0.05*P3(error_r, kpr);
+ }
+
+ if (motorValue3 <=0) motor3DirectionPin=0;
+ else motor3DirectionPin=1;
+ if (fabs(motorValue3)>1) motor3MagnitudePin = 1;
+ else motor3MagnitudePin = fabs(motorValue3);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
// einde deel motor------------------------------------------------------------------------------------
-
+
Ticker loop;
-
-/*Calculates ropelengths that are needed to get to new positions, based on the
+
+/*Calculates ropelengths that are needed to get to new positions, based on the
set coordinates and the position of the poles */
-float touwlengtes(){
- Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));
+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));
return 0;
}
-
-/* Calculates rotations (and associated counts) of the motor to get to the
+
+/* Calculates rotations (and associated counts) of the motor to get to the
desired new position*/
-float turns(){
-
- roto=Lou/omtrekklosje;
- rotb=Lbu/omtrekklosje;
- rotr=Lru/omtrekklosje;
- counto=roto*4200;
+float turns()
+{
+
+ roto=Lou/omtrekklosje;
+ rotb=Lbu/omtrekklosje;
+ rotr=Lru/omtrekklosje;
+ counto=roto*4200;
dcounto=counto-hcounto;
//pc.printf("counto = %f \n\r", counto);
//pc.printf("hcounto = %f \n\r", hcounto);
@@ -473,115 +608,129 @@
//pc.printf("dcountb = %f \n\r",dcountb);
countr=rotr*4200;
dcountr=countr-hcountr;
-
+
return 0;
}
-
-// Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
-float Pst(){
+
+// Waar komen Pstx en Psty vandaan en waar staan ze voor? En is dit maar voor een paal?
+float Pst()
+{
Pstx=Pex+Vex*T;
Psty=Pey+Vey*T;
touwlengtes();
- Pex=Pstx;
+ Pex=Pstx;
Pey=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("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);
/*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);*/
return 0;
-}
-
+}
+
//Calculating desired end position based on the EMG input <- Waarom maar voor een paal?
-float Ps(){
+float Ps()
+{
Psx=(Xin_new)*30+91;
- Psy=(Yin_new)*30+278;
- // pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
+ Psy=(Yin_new)*30+278;
+// pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
return 0;
}
-
+
// Rekent dit de snelheid uit waarmee de motoren bewegen?
-void Ve(){
+void Ve()
+{
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;
- }*/
+ /* 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)){
+// pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
+ if((fabs(Vex)<0.01f)&&(fabs(Vey)<0.01f)) {
Move_done=true;
loop.detach();
- }
+ }
}
-
+
// Calculating the desired position, so that the motors can go here
-int calculator(){
+int calculator()
+{
+ if(Tiller==true){
Ps();
if (Move_done == false) {
- loop.attach(&Ve,0.02);
+ 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){
+void zakker()
+{
+ while(1) {
wait(1);
- if(Move_done==true){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is
- dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
- dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
- dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
- rotzo=dLod/omtrekklosje;
- rotzb=dLbd/omtrekklosje;
- rotzr=dLrd/omtrekklosje;
- countzo=rotzo*4200;
- countzb=rotzb*4200;
- countzr=rotzr*4200;
-
- //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
- }
+ if(Move_done==true) { //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is
+ 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));
+ dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
+ dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
+ dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
+ rotzo=dLod/omtrekklosje;
+ rotzb=dLbd/omtrekklosje;
+ rotzr=dLrd/omtrekklosje;
+ countzo=rotzo*4200;
+ countzb=rotzb*4200;
+ countzr=rotzr*4200;
+
+ //pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
+ }
}
}
-void tiller(){
- //int reference_o = hcounto-12487;
- //int reference_b = hcountb-8148;
- //int reference_r = hcountr-7386;
+void tiller()
+{
pc.printf("Tiller");
-/* Vex = 20;
- Vey = 20;*/
- controlmotor1.attach(&MotorController1, 0.01);
- controlmotor2.attach(&MotorController2, 0.01);
- controlmotor3.attach(&MotorController3, 0.01);
- }
+ /* Vex = 20;
+ Vey = 20;*/
+ controlmotor1.attach(&MotorController1t, 0.01);
+ controlmotor2.attach(&MotorController2t, 0.01);
+ controlmotor3.attach(&MotorController3t, 0.01);
+}
-void setcurrentposition(){
- if(Input_done==true){
+
+void setcurrentposition()
+{
+ if(Input_done==true) {
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);
pc.printf("ik reset hcounts");
Input_done=false;
- }
}
-
+}
+
int main()
{
pc.baud(115200);
wait(1.0f);
- tiller();
getbqChain();
threshold_timerR.attach(&Threshold_samplingBR, 0.002);
threshold_timerL.attach(&Threshold_samplingBL, 0.002);
setcurrentposition();
- while(true){
+ tiller();
+ wait(10);
+ int reference_o=0;
+ int reference_b=0;
+ int reference_r=0;
+ while(true) {
sample_timer.attach(&EMG_sample, 0.002);
wait(2.5f);
tellerX();
@@ -592,7 +741,6 @@
controlmotor3.attach(&MotorController3, 0.01);
//zakker();
wait(5.0f);
- }
+ }
}
-
\ No newline at end of file