File content as of revision 5:4530cd699129:

#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); //D10 to ENC1 A
DigitalIn enc_1b(D9); //D9 to ENC1 B
QEI Encoder_1(D10,D9,NC,64,QEI::X4_ENCODING); 

//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 angle_1; //input wanted angle 
volatile float psi_1 = 0; //angle of motor_1 in radians
volatile float psiabs_1 = 0; //absolute value of psi_1
#define M_PI acos(-1.0)
const float to_rads =(2*M_PI); //2*Pi
volatile int counts_1 = 0; // counts of Encoder_1
const float CountsPerRevolution = 8400; // counts of the encoder per revolution (Property of motor)
//const float GearRatio = 131.25; // GearRatio(Property of motor)
const float ShaftResolution = (to_rads)/(CountsPerRevolution); // calculates constant for relation between counts and radians (rad/count)

// FUNCTIONS
float GetWidthFromKeyboard() //Gets input from PC
{
    start_width:
    
    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_width;
        }
    else if(a<-1)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
        goto start_width;
        }
    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;
        }
}

float GetAngleFromKeyboard() //Gets input from PC
{
    start_angle:
    
    float a;
    cin >> a;
    if(a>to_rads)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
        goto start_angle;
        }
    else if(a<0)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (-1,1).\r\n");
        goto start_angle;
        }
    else
    {
        pc.printf("\nYour input: [%f]\r\n\r\n--------\r\n\n",a);
        return a;
        }
}

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); 
    }

float Enc_1psi() //calculates the angle of motor 1
{
    psi_1 = ShaftResolution * counts_1;
    pc.printf("Angle of motor_1: %f radians\r\n",psi_1);
    psiabs_1 = abs(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.01); //Ticker function to read Enc_1 to HIDScope
    
    start_main:
    pc.printf("counts_1 = %i\r\n", counts_1);
    
    wait(0.5);
    
    pc.printf("Please imput your <Angle> value (0,2pi)\r\n");
    angle_1 = GetAngleFromKeyboard(); //(0,2pi)
    
    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 = GetWidthFromKeyboard(); //in percentile (-1,1)
    
    direction_1 = dir_1;
    PWM_MOTOR_1();      
    
    while (true) 
    {
        counts_1 = Encoder_1.getPulses(); 
        Enc_1psi();
        MOTOR_1();
        if (psiabs_1 > angle_1)
        {
            pc.printf("--------\r\nReached Destination\r\n--------\r\n\n");
            goto start_main;
            }
        if (but_1 == 0)
        {
            pc.printf("--------\r\nTurning off motor...\r\n--------\r\n\n");
            goto start_main;
            }
    }
}