File content as of revision 4:050dd85ffcf5:

#include "mbed.h"
#include "math.h"
#include "encoder.h"
#include "QEI.h"

DigitalIn button1(D8);
DigitalIn button2(D9);
//AnalogIn potMeterIn(A1);
float potMeterIn; // = 0.01; snelheid in, zonder potmeter gebruik

DigitalOut motor1DirectionPin(D4);
PwmOut motor1MagnitudePin(D5);

QEI Encoder(D12,D13,NC,32);


int counts = Encoder.getPulses();
int delta;
DigitalOut led(LED_RED);
DigitalOut ledd(LED_GREEN);
DigitalOut leddd(LED_BLUE);
InterruptIn button(SW2);
Timer t;
Serial pc(USBTX, USBRX);
int Xin=0;
float huidigetijdX;
void ledtX(){
    t.reset();
    Xin++;
    pc.printf("Xin is %i\n",Xin);
    ledd=0;
    led=1;
    wait(0.5);
    ledd=1;
    led=0;
}  

int tellerX(){
    ledd=1; 
    leddd=1;
    pc.baud(115200);
    while(true){
    button.fall(ledtX);      
    /*if (EMG>threshold){
        Piek();                 // dit is wat je uiteindelijk wil dat er staat
    }*/
    t.start();
    huidigetijdX=t.read();
    if (huidigetijdX>2){
        led=1;          // ga door naar het volgende programma
        return 0;
        }
    }  
}   
int Yin=0;
float huidigetijdY;
void ledtY(){
    t.reset();
    Yin++;
    pc.printf("Yin is %i\n",Yin);
    ledd=0;
    leddd=1;
    wait(0.5);
    ledd=1;
    leddd=0;
}  

int tellerY(){
    t.reset();
    ledd=1;
    leddd=0; 
    pc.baud(115200);
    while(true){
    button.fall(ledtY);      
    /*if (EMG>threshold){
        Piek();                 // dit is wat je uiteindelijk wil dat er staat
    }*/
    t.start();
    huidigetijdY=t.read();
    if (huidigetijdY>2){
        leddd=1; 
        button.fall(0);   
        return 0;      // ga door naar het volgende programma 
        }
    }
}
int B;
float Pox = 0;
float Poy = 0;
float Pbx = 0;
float Pby = 887;
float Prx = 768;
float Pry = 443;
float Pex=121;
float Pey=308;
float diamtrklosje=20;
float pi=3.14159265359;
float omtrekklosje=diamtrklosje*pi;
float Lou;
float Lbu;
float Lru;
float dLod;
float dLbd;
float dLrd;
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 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


//Deel om motor(en) aan te sturen--------------------------------------------

void calcdelta(void) {    
    delta = (counto - Encoder.getPulses());
    }

void FeedBack(void)
    { 
    if (delta > 100)
        {   potMeterIn = -0.1;
        }
    if (delta < -100)
        {   potMeterIn = 0.1;
        } 
    else 
        {   potMeterIn = 0;
        }
     }   

//Encoder motor1(D12,D13);    //Gear ratio = encoder ratio = 131.25:1 & 
// factor 64: To compute the counts per revolution of the gearbox output, multiply the gear ratio by 64.

float referenceVelocity; 
float motorValue; 

Ticker controlmotor1; // één ticker van maken?
Ticker calculatedelta; 
Ticker feedbacktick; 
Ticker printdata;     //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes

float GetReferenceVelocity()
{
    // Returns reference velocity in rad/s. 
    // Positive value means clockwise rotation.
    float maxVelocity=Vex*25+Vey*25; // max 8.4 in rad/s of course!   
    
    if (button1)   
        {
        // Clockwise rotation      
        referenceVelocity = -1*potMeterIn * maxVelocity;
        } 
        else 
        {
        // Counterclockwise rotation       
        referenceVelocity = potMeterIn * maxVelocity;  
        }
    
    if (button2)
        {   potMeterIn = potMeterIn*1;
        }
        else
        {   potMeterIn = potMeterIn*0;
        }
    
    if (Encoder.getPulses() < counto)
        {   potMeterIn = 1;
        }
        else
        {   potMeterIn = 0;
        }
        
    return referenceVelocity;
}         


void SetMotor1(float motorValue)
{
    // 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 (motorValue >=0) motor1DirectionPin=1;
        else motor1DirectionPin=0;
    if (fabs(motorValue)>1) motor1MagnitudePin = 1;
        else motor1MagnitudePin = fabs(motorValue);
      
}

float FeedForwardControl(float referenceVelocity)
{
    // very simple linear feed-forward control
    const float MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
    float motorValue = referenceVelocity / MotorGain;
    return motorValue;  
}
        
void MeasureAndControl(void)
{
    // 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.
    float referenceVelocity = GetReferenceVelocity();
    float motorValue = FeedForwardControl(referenceVelocity);
    SetMotor1(motorValue);
}

void readdata(void)
    {   //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
        pc.printf("Pulses = %i \r\n",Encoder.getPulses());
        //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
        pc.printf("Delta = %i \r\n",delta);
    }

// einde deel motor------------------------------------------------------------------------------------

Ticker loop;
float touwlengtes(){
    Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2));     // rekent touwlengtes uit die nodig zijn voor de nieuwe positie aan de hand van de ingegeven coördinaten en de posities van de palen
    Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2));
    Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
    return 0;
}
float turns(){
    
    roto=Lou/omtrekklosje; 
    rotb=Lbu/omtrekklosje; 
    rotr=Lru/omtrekklosje; 
    counto=roto*4200; 
    //counto = (int)(counto + 0.5);                      // omzetten van rotaties naar counts
    countb=rotb*4200;
    //countb = (int)(countb + 0.5);
    countr=rotr*4200;
    //countr = (int)(countr + 0.5);
    return 0;
}

    
float Pst(){
    Pstx=Pex+Vex*T;
    Psty=Pey+Vey*T;
    touwlengtes();
    Pex=Pstx;                   //Dit is nog een aanname. Moet zijn: read the encoder and put the real position as the Pe for the next step
    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("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 constand is en de end efector dus een rechte lijn maakt
    pc.printf("\n\r R=%f",R);*/
    return 0;
} 
 
float Ps(){
    Psx=(Xin)*30+121;
    Psy=(Yin)*30+308;     
    pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
    return 0;
}
void Ve(){
    Vex=Kz*(Psx-Pex);
    Vey=Kz*(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((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
        B=5;
        loop.detach();
        }
}

int calculator(){
    pc.baud(115200);
    Ps();
    loop.attach(&Ve,0.02);
    return 0;
}

void zakker(){
    while(1){
        wait(1);
    if(B==5){                                   // 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
    }
    }
}

int main()
{
    tellerX();
    tellerY();
    calculator();
    controlmotor1.attach(&MeasureAndControl, 0.01);
    calculatedelta.attach(&calcdelta, 0.01);
    if (abs(delta) < -50)               // dit moet denk ik in een ticker staan dus bij calcdelta in denk ik, moeten we maar ff proberen!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
       { feedbacktick.attach(&FeedBack, 0.01);}
    printdata.attach(&readdata, 0.5);  
    //zakker();
        
    return 0;
}