// Tensy 3.2 QEI, USBSerial, MotCon example
// J. Bradshaw - 20180111

#include "mbed.h"
#include "USBSerial.h"
#include "MotCon.h"
#include "QEI.h"

QEI enc1(D2,D3,NC,1024, QEI::X2_ENCODING);
MotCon mot(D4, D5); //pwm, dir
USBSerial pc;
Serial ser1(D1,D0); // tx, rx
DigitalOut led1(LED1);
AnalogIn current(A0);   

int main() {
    wait(2.0);
    //pc.baud(9600);  //NO BAUD RATE FOR USBSerial object    
    pc.printf("\r\n%s\r\n", __FILE__); //display the filename (this source file)
    //pc.printf("%s %s\n",__TIME__,__DATE__);
    pc.printf("Version 1.0 - rev %s J. Bradshaw\r\n", __DATE__);
    
    enc1.reset();
    
    while(1) {
         for(float p=0; p<2.0*3.14159; p += 0.01) {
            float pwm_dc = sin(p);
            mot.mot_control(pwm_dc);
            float encoder1 = (float)enc1.getPulses();                                                  
            //pc.printf("%7.2f %7.2f %5.3f\r\n", pwm_dc, encoder1, current.read());
            led1 = !led1;
            
            wait(0.005);            
        }
    }
}