File content as of revision 0:3c2e28fef203:

#include "mbed.h"
//#include "HIDScope.h"
//#include "QEI.h"
#include "MODSERIAL.h"
//#include "BiQuad.h"
#include "FastPWM.h"
#include <iostream>
#include <math.h>

//COMMUNICATING WITH PC
#define SERIAL_BAUD 115200
Serial pc(USBTX, USBRX);

Ticker EncTicker; // Ticker for encoder

//MOTOR 1 //DONT HAVE TO BE CONNECTED BY WIRES, IS DONE BY THE HARDWARE ITSELF
DigitalOut direction_1(D7); //tells motor 1 to turn CW (bool = false) or CCW (bool = true)
PwmOut msignal_1(D6); //PWM signal to motor controller, higher PWM = higher avarage voltage
DigitalIn but_1(D1); //D1 to BUT1
DigitalIn enc_1a(D10); //D13 to ENC1 A
DigitalIn enc_1b(D9); //D12 to ENC1 B

//MOTOR 2
//DigitalOut direction_2(D4); //tells motor 2 to turn CW or CCW
//PwmOut msignal_2(D5); //PWM signal to motor controller, higher PWM = higher avarage voltage

volatile float width;
volatile float width_percent;
float period = 10.0; //sets period to 10ms
volatile float on_time_1;
volatile float off_time_1;
volatile bool dir_1; //True = CCW, false = CW
volatile float a_1 = enc_1a; // enc_a1 signal
volatile float b_1 = enc_1b; // enc b_1 signal
volatile float a_1prev = enc_1a; // enc_a1 previous signal
volatile float b_1prev = enc_1b; // enc b_1 previous signal
volatile int ticks_1 = 0; //counts times the encoder ticked from the start position
volatile float psi_1 = 0; //angle of motor_1 in radians
volatile float psi_1prev = 0; //precious angle of motor_1 in radians
#define M_PI acos(-1.0)
const float to_rads =((2*M_PI)/8400); //contant for claculating one tick of the motor to radians, 8400 is a property of the motor -> 8400 counts per revolution

// FUNCTIONS
float GetValueFromKeyboard() //Gets input from PC
{
    start:
    
    float a;
    cin >> a;
    if(a>1)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
        goto start;
        }
    else if(a<-1)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
        goto start;
        }
    else if(a<0)
    {
        pc.printf("\nYour input: [%f] -> CW motion\r\n\r\n--------\r\n\n",a);
        dir_1 = false;
        a = abs(a); //otherwise width becomes negative, and it doesnt like that
        return a;
        }
    else
    {
        pc.printf("\nYour input: [%f] -> CCW motion\r\n\r\n--------\r\n\n",a);
        dir_1 = true;
        return a;
        }
}

//void GetDirectionFromkeyboard ()
//{
//    start_dir:
//    
//    int b;
//    cin >> b;
//    switch (b)
//    {
//        case 1:
//        {
//            pc.printf("\nYour input: [1] -> CCW motion\r\n\r\n--------\r\n\n");
//            dir_1 = true;
//            break;
//            }
//        case 0:
//        {
//            pc.printf("\nYour input: [0] -> CW motion\r\n\r\n--------\r\n\n");
//            dir_1 = false;
//            break;
//            }
//        default:
//        {
//            pc.printf("\nYour input was invalid, please input 1 for CCW or 0 for CW motion.\r\n");
//            goto start_dir;
//            }
//    }
//}

void PWM_MOTOR_1 (void) //Calculates on and off times
{
    width = period * width_percent;
    on_time_1 = width;
    off_time_1 = period - width;
    }

void MOTOR_1 () //runs the motor
{
    msignal_1 = 1; // Turn motor on
    wait_ms(on_time_1);
    msignal_1 = 0; // Turn motor off
    wait_ms(off_time_1); 
    }
    
void ReadEnc_1() 
{ 
    a_1prev = a_1;
    b_1prev = b_1;
    a_1 = enc_1a;
    b_1 = enc_1b;

    //pc.printf("a_1i = %f, b_1i = %f\r\n", a_1, b_1);
    }

float Enc_1Radians ()
{
    if(a_1==a_1prev && b_1==b_1prev)
    {
        }
    else
    {
        psi_1prev = psi_1;
        if(dir_1)
        {
            psi_1 = psi_1prev + to_rads;
            }
        else
        {
            psi_1 = psi_1prev - to_rads;
            }
        pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
        }
    return psi_1;
    }

int main()
{   
    pc.baud(SERIAL_BAUD);
    
    pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
    EncTicker.attach(&ReadEnc_1, 0.0001); //Ticker function to read Enc_1 to HIDScope
    
    start_main:
    
//    pc.printf("Please imput your desired direction value, either 1 for CCW or 0 for CW movement\r\n");
//    GetDirectionFromkeyboard();
     
    
    wait(0.5);
    
    pc.printf("Please imput your <width> value in percentile of the period (-1,1), positive for CCW motion or negative for CW motion\r\n");
    width_percent = GetValueFromKeyboard(); //in percentile (-1,1)
    direction_1 = dir_1;
    PWM_MOTOR_1();      
    
    while (true) 
    {
        Enc_1Radians ();
        MOTOR_1();
        if (but_1 == 0)
        {
            pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
            goto start_main;
            }
    }
}