File content as of revision 7:e871e2a28f08:

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

//COMMUNICATING WITH PC
#define SERIAL_BAUD 115200
#define M_PI acos(-1.0)

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

const float period = 10.0; //sets period to 10ms
const float to_rads =(2*M_PI); //2*Pi
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)
volatile float width;
volatile float width_percent;
volatile float on_time_1;
volatile float off_time_1;
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
volatile int counts_1 = 0; // counts of Encoder_1
volatile bool dir_1; //True = CCW, false = CW

// 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 (0,1).\r\n");
        goto start_width;
        }
    else if(a<0)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (0,1).\r\n");
        goto start_width;
        }
    else
    {
        pc.printf("\nYour input: [%f]\r\n\r\n--------\r\n\n",a);
        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 (-2pi,2pi).\r\n");
        goto start_angle;
        }
    else if(a< -to_rads)
    {
        pc.printf("\nYour input was invalid, please imput a value in interval (-2pi,2pi).\r\n");
        goto start_angle;
        }
    else if(a<0)
    {
        pc.printf("\nYour input: [%f] --> CW motion.\r\n\r\n--------\r\n\n",a);
        dir_1 = false;
        return a;
        }
    else
    {
        pc.printf("\nYour input: [%f] --> CCW motion.\r\n\r\n--------\r\n\n",a);
        dir_1 = true;
        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);
    return psi_1;
    }

int main()
{   
    pc.baud(SERIAL_BAUD);
    
    pc.printf("--------\r\nWelcome!\r\n--------\r\n\n");
    
    start_main:
    pc.printf("counts_1 = %i\r\n", counts_1); //For own insight, not necessary in final code
    
    wait(0.5);
    
    pc.printf("Please imput your <Angle> value (-2pi,2pi), positive for CCW motion or negative for CW motion\r\n");
    angle_1 = GetAngleFromKeyboard(); //(-2pi,2pi)
    
    wait(0.5);
    
    pc.printf("Please imput your <width> value in percentile of the period (0,1)\r\n");
    width_percent = GetWidthFromKeyboard(); //in percentile (0,1)
    
    direction_1 = dir_1;    //give direction to port
    PWM_MOTOR_1();      
    
    while (true) 
    {
        counts_1 = Encoder_1.getPulses(); 
        Enc_1psi();
        MOTOR_1();
        if (dir_1) // stop for CCW motion
        {
            if (psi_1 > angle_1)
            {
                pc.printf("--------\r\nReached Destination\r\n--------\r\n\n");
                goto start_main;
                }
            }
        if (!dir_1) // stop for CW motion
        {
            if (psi_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;
            }
    }
}