File content as of revision 10:d78f5d556cf7:

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
#define _USE_MATH_DEFINES
# define M_PI           3.14159265358979323846  /* pi */

DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);

PwmOut  motor1_pwm(D5);
PwmOut  motor2_pwm(D6);
DigitalOut motor1_dir(D4);
DigitalOut motor2_dir(D7);
AnalogIn pot1(A0);
AnalogIn pot2(A1);

QEI motor1_enc(D12,D13,NC,8400,QEI::X4_ENCODING);
QEI motor2_enc(D10,D11,NC,8400,QEI::X4_ENCODING);  

InterruptIn  sw2(SW2);
InterruptIn  sw3(SW3);

MODSERIAL pc(USBTX, USBRX);

Ticker      sample_timer;

const float sample_time = 0.05; //s
const float sample_frequency = 200; //hz


double countstoangle(int counts){
    double angle;
    int counts_abs = abs(counts);
    if(counts_abs >= 8400){
       int partial_counts = counts_abs % 8400;
       angle = (2 * M_PI * partial_counts)/8400;
       return angle;
    }else{
        angle = (2 * M_PI * counts_abs)/8400;
        return angle;
    }
}


// KINEMATICS
// Variables
const int L1 = 300; // Length arm 1 in mm
const int L2 = 482; // Length arm 2 in mm

// Gets current postition of end effector | FOWARD KINEMATICS
double getX1pos(double q1){ // X-pos of joint
    return x1_c = L1*cosd(q1);
}
double getY1pos(double q1){ // Y-pos of joint
    return y1_c = L1*sind(q1);
}
double getX2pos(double q1, double q2){ // X-pos of end-effector
    double x1_c = getX1pos(q1)
    return x2_c = x1_c + L2*cosd(q1+q2);
}
double getY2pos(double q1, double q2){ // Y-pos of end-effector
    double y1_c = getY1pos(q1)
    return y2_c = y1_c + L2*sind(q1+q2);
}

// Calculate new position | BACKWARD KINEMATICS
double getNewq2(double x2_d, double y2_d){ // requires desired x and y position of end-effector
    double frac = ( pow(x2_d,2) + pow(y2_d,2) - pow(L1,2) - pow(L2,2) ) / ( 2 * L1 * L2 ); // define kinematic cosine fraction for easier calculation
    double q2_d = atan2d( -sqrt(1 - pow(frac,2)) , frac ); // new desired q2 joint angle
    return q2_d;
}
double getNewq1(double x2_d, double y2_d){ // requires desired x and y position of end-effector
    double q2_d = getNewq2(x2_d, y2_d); // first calculate q2
    double k1 = L1 + L2*cosd(q2_d); // kinematic relation 1
    double k2 = L2*sind(q2_d); // kinematic relation 2
    double q1_d = atan2d( y2_d, x2_d ) - atan2d( k2, k1); // new desired q1 joint angle
    return q1_d;
}
double getNewq3(double x2_d, double y2_d){ // Cup holder
    double q3_d = getNewq2(x2_d, y2_d) + getNewq1(x2_d, y2_d); // q2_d+q1_d
}






int main()
{
    ledr = 1;
    ledg = 1;
    ledb = 1;
    
    pc.printf("\r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n ~~~A$$De$troyer69~~~ \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n \r\n");
    
    motor1_pwm.period_us(60);
    motor1_pwm = 0;  
    motor2_pwm.period_us(60);
    motor2_pwm = 0;  
  
    while (true)
    {
            float pot1_float = pot1.read();
            float pot1_motor = (pot1_float * 2.0f) - 1.0f;
            int mot1_direction = pot1_motor >= 0;
            motor1_pwm.write(fabs(pot1_motor));
            motor1_dir = !mot1_direction;
            
            float pot2_float = pot2.read();
            float pot2_motor = (pot2_float * 2.0f) - 1.0f;
            int mot2_direction = pot2_motor >= 0;
            motor2_pwm.write(fabs(pot2_motor));
            motor2_dir = mot2_direction;
            
            double motor1_ang = countstoangle(motor1_enc.getPulses());
            double motor2_ang = countstoangle(motor2_enc.getPulses());
            
            pc.printf("Angle motor 1: %f pi rad, angle motor 2: %f pi rad.\r\n", motor1_ang/(M_PI), motor2_ang/(M_PI));
            
            wait(0.2f);
    }
}