File content as of revision 36:331907080ffb:

//voltage in PWM (between -1 and 1)
//period in seconds
//dt the time between measurements, i.o.w. Ticker timing 

#include "mbed.h"
#include "FastPWM.h"
#include "QEI.h"
#include "global.h"

//Objects
    //Motors
        FastPWM motor1(D6); //Motor1 PWM output pin
        DigitalOut motor1Dir(D7); //Motor1 directional pin
        FastPWM motor2(D5); //Motor2 PWM output pin
        DigitalOut motor2Dir(D4); //Motor2 directional pin
        //FastPWM motor3(); //Motor3 PWM output pin
        //DigitalOut motor3Dir(); //Motor3 directional pin
    //Encoders
         QEI encoderMotor1(D12,D13,NC,64,QEI::X2_ENCODING);
         QEI encoderMotor2(D12,D13,NC,64,QEI::X2_ENCODING);
         //QEI encoderMotor3(,,NC,64,QEI::X2_ENCODING);

    //Variables
        static int countsMotor1[2]; //Array to store motor counts for i and i-1
        static int countsMotor2[2];
        static int countsMotor3[2];
        static float velocityMotor1;
        static float velocityMotor2;
        static float velocityMotor3;
        static float angleMotor1;
        static float angleMotor2;
        static float angleMotor3;
    //Constants
        const int uniqueMeasurementPoints = 4200; //From Canvas X4 = 8400, X1 = 2100, so X2 = 4200 https://canvas.utwente.nl/courses/4023/pages/project-materials?module_item_id=91422
        const double PI = 3.14159265358979;
        const float countsToRadians = (2*PI)/uniqueMeasurementPoints; //Number of radians per count
        
        motorData motorReturn;

void motorAndEncoder(float PWM1, float PWM2, float PWM3, float dt)
{
    //Set motors
        //Direction
            //Motor 1
                if (PWM1<0)
                {
                    PWM1 *= -1; //Shorthand for *-1
                    motor1Dir.write(1); //negative direction
                }
                else
                {
                    motor1Dir.write(0); //positive direction    
                }
            //Motor 2
                if (PWM2<0)
                {
                    PWM2 *= -1; //Shorthand for *-1
                    motor2Dir.write(1); //negative direction
                }
                else
                {
                    motor2Dir.write(0); //positive direction    
                }
            //Motor 3
                if (PWM3<0)
                {
                    PWM3 *= -1; //Shorthand for *-1
                   // motor3Dir.write(1); //negative direction
                }
                else
                {
                    //motor3Dir.write(0); //positive direction    
                }
        //Period and PWM
            float periodPWM = 1/2000;
            motor1.period(periodPWM);
            motor1.write(PWM1);
        
            motor2.period(periodPWM);
            motor2.write(PWM2);
            
            //motor3.period(periodPWM);
            //motor3.write(PWM3);
            
    //Read encoders
    
        //Counts
            //set the value from last loop to poisition 0 in the vector
                countsMotor1[0] = countsMotor1[1];
                countsMotor2[0] = countsMotor2[1];
                //countsMotor3[0] = countsMotor3[1];
            //set the new number of counts to position 1 in the vector
                countsMotor1[1] = encoderMotor1.getPulses();
                countsMotor2[1] = encoderMotor2.getPulses();
                //countsMotor3[1] = encoderMotor3.getPulses();
        //Angle
            angleMotor1 = countsMotor1[1]/countsToRadians;
            angleMotor2 = countsMotor2[1]/countsToRadians;   
            //angleMotor3 = countsMotor3[1]/countsToRadians;       
        //Velocity calculation
            velocityMotor1 = ((countsMotor1[1]-countsMotor1[0])/countsToRadians)/dt; //difference in counts difided by the number of counts per radians [rad/s]
            velocityMotor2 = ((countsMotor2[1]-countsMotor2[0])/countsToRadians)/dt; //rad/s
            //velocityMotor3 = ((countsMotor3[1]-countsMotor3[0])/countsToRadians)/dt; //rad/s
    
    //Push variables to global structure
        ::motorReturn.motor1.counts = countsMotor1[1]; //:: To signal the global structure
        ::motorReturn.motor2.counts = countsMotor2[1];
        //::motorReturn.motor3.counts = countsMotor3[1];
        
        ::motorReturn.motor1.angle = angleMotor1;
        ::motorReturn.motor2.angle = angleMotor2;
        //::motorReturn.motor3.angle = angleMotor3;
        
        ::motorReturn.motor1.velocity = velocityMotor1;
        ::motorReturn.motor2.velocity = velocityMotor2;
        //::motorReturn.motor3.velocity = velocityMotor3;
    
}