Code om de PID controller af te stellen aan de hand van een sinus golf

Dependencies:   mbed QEI MODSERIAL FastPWM biquadFilter

main.cpp

Committer:
aschut
Date:
2019-03-15
Revision:
4:99f7fdce608e
Parent:
3:ac13255164cd
Child:
5:4b25551aeb6e

File content as of revision 4:99f7fdce608e:

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

// Algemeen
DigitalIn button2(SW2);
DigitalIn button3(SW3);
DigitalOut led(LED_GREEN);
DigitalOut led2(LED_RED);
DigitalOut led3(LED_BLUE);
MODSERIAL pc(USBTX, USBRX);

//Motoren
DigitalOut direction1(D4);
PwmOut pwmpin1(D5);
PwmOut pwmpin2(D6);
DigitalOut direction2(D7);
volatile float PWM1;
volatile float PWM2;
volatile float pwm2;

//Encoder
DigitalIn EncoderA(D13);
DigitalIn EncoderB(D12);
QEI encoder2 (D13, D12, NC, 8400, QEI::X4_ENCODING);
float Pulses2;
float Degrees2;

//Pot meter
AnalogIn pot(A1);
float Pot2;

//Ticker
Ticker Pwm;
Ticker PotRead;






void Period(void)
    {
    pwmpin2.period_us(60);
    }
    
void PwmMotor(void)
    {     
    pwm2 = (Pot2*2)-1;                  //scaling
    pc.printf("pwm2=%f\r\n",pwm2);
    Degrees2 = (Pulses2/8400)*360;
    pc.printf("Pulses is: %i\n", Pulses2);
    direction2 = pwm2 < 0.0f;    //positief = CW, negatief = CCW
    pwmpin2 = fabs(pwm2);
    

    }  
void MotorOn(void)
    {
    pwmpin2 = 0;
    Pwm.attach (PwmMotor, 0.1);
    }
void MotorOff(void)
    {
    Pwm.detach ();    
    pwmpin2 = 0;
    }

void ContinuousReader(void){
    Pot2 = pot.read();
    Pulses2 = encoder2.getPulses();
    //pc.printf("%f\r\n",Pot2);
    }
    

       
int main() {
    Period();
    PotRead.attach(ContinuousReader,0.1);
    pc.baud(115200);
    pc.printf("start\r\n");
    led = 1;
    led2 =1;
    led3 =1;
    
    while (true){
    led3 = 0;
    if (!button2)
    {
        led3 = 1;
        led = 0;
        pc.printf("MotorOn\r\n");
        MotorOn();   
    }
    if (!button3)
    {
        pc.printf("MotorOff\r\n");
        PotRead.detach();
        MotorOff();
    }
    led = 0;
        }
    }