File content as of revision 8:61a3a5fb4c37:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"

AnalogIn  potmeter1(PTC10);
//AnalogIn  potmeter2(PTC11);
MODSERIAL  pc(USBTX, USBRX);
//D4 is a digital input for the microcontroller, so should be an digitalOut
//from the K64F. It will tell the motor shiel to let Motor1 turn clockwise
//of count clockwise (CW of CCW). D4 for motor 2
DigitalOut directionM1(D4);
//D5 is a PWM input for the motor controller and determines the PWM signal 
//that the motor controller gives to Motor 1. Higher PWM, higer average voltage.
// D6 for motor 2
FastPWM motor1_pwm(D5);
// For Encoder reading
QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)

// Voor het laten zien van de data op de pc.
HIDScope scope(1);              // Aantal kanalen wat doorgegeven wordt aan de hidscope
Ticker AInTicker;
Ticker Waardesmotor;

// Declare variables for motor
float pos;
float potwaarde1;
//float potwaarde2;
const float pi = 3.14159265359;
int counts1; 
float theta1;
float error1;
float U1;



void ReadAnalogIn()
{
    scope.set(0,pos);    // Zet de potwaarde in de eerste plot bij de HID scope. Deze wordt automatisch tegen de tijd geplot
    scope.send();               // Zendt de waardes naar de pc 
    }

float ReadEncoder()
{
    // Get counts
    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1

    // Get angles
    theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1

    //pc.printf("Hoek motor 1 = %0.2f rad \n \r", theta1);
    return theta1; 
}

/**
float P_controller(float error)
{
    float Kp = 2.0;
    float U1 = Kp*error;
    return U1;  
}
*/

void Waardes()
{
    pc.printf("Hoek motor 1 = %0.2f rad, hoek potmeter = %0.2f rad, error = %0.2f rad, U1 = %0.2f \n \r", theta1, pos, error1,U1);
}
    

int main(void)
{
    pc.baud(115200);
    motor1_pwm.period_us(60);               // Period is 60 microseconde
    AInTicker.attach(&ReadAnalogIn,0.001f);  // Elke 0.001 sec. Lees de analoge waarde
    Waardesmotor.attach(Waardes,1);     // Elke 1 sec wordt de waarde van motor 1 geplot

    while(true){
        potwaarde1 = potmeter1.read();  // Lees de potwaardes uit. Tussen 0 en 1
        //potwaarde2 = potmeter2.read();
        
                    
        pos = (potwaarde1*2 - 1) * 2*pi;   // Scale van -1 tot 1 ipv. 0 tot 1
        theta1 = ReadEncoder();
        error1 = pos - theta1;
        float Kp = 1;
        U1 = Kp*float(error1)/6.28;

        //float U1 = P_controller(error);
        
        // Gebruik de absolute waarde van de scaled U waardes als input voor de motor. 
        // Negatieve waardes kunnen niet naar de motor gestuurd worden.
        motor1_pwm.write(fabs(U1)); 
        
        directionM1 = U1 > 0.0f; //either true or false
        
        wait(0.002f);   
     }
    
}