Hier zitten extra leipe functies in naast PID ook x en y naar hoeken

Dependencies:   QEI MODSERIAL

main.cpp

Committer:
JonaVonk
Date:
2019-10-04
Revision:
0:6986a58c4515
Child:
1:363c5230fe25

File content as of revision 0:6986a58c4515:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

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


DigitalOut M1(D4);
DigitalOut M2(D7);
PwmOut E1(D5);
PwmOut E2(D6);
AnalogIn Pot1(A0);
AnalogIn Pot2(A1);
DigitalIn M1A(D2);
DigitalIn M1B(D3);

float potVal1;
float potVal2;
int EncA1;
int EncB1;
int counts = 0;

//Ticker readEnc

MODSERIAL pc(USBTX, USBRX);




// main() runs in its own thread in the OS
int main(){
    pc.baud(9600);
    while(true){
        potVal1 = Pot1.read();
        potVal2 = Pot2.read();
        if (potVal1 > 0.5){
            M1 = 1;
            E1 = potVal1-0.5;
        }else{
            M1 = 0;
            E1 = -(potVal1-0.5);
        }
        if (potVal2 > 0.5){
            M2 = 1;
            E2 = potVal2-0.5;
        }else{
            M2 = 0;
            E2 = -(potVal2-0.5);
        }
        pc.printf("Pot1: %f \t Pot2: %f \n\r", potVal1, potVal2);
    }
}