File content as of revision 4:5f147787c2ca:

/* mbed Microcontroller Library
 * Copyright (c) 2018 ARM Limited
 * SPDX-License-Identifier: Apache-2.0
 */

#include "mbed.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"

QEI Encoder(D12,D13,NC,32);

DigitalOut M1(D4);
DigitalOut M2(D7);

PwmOut E1(D5);
PwmOut E2(D6);


MODSERIAL pc(USBTX, USBRX);


void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes);

// main() runs in its own thread in the OS
int main()
{
    float steps = 100;
    pc.baud(115200);
    while (true) {
        for(int i = 0; i < steps; i++) {
            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
            //pc.printf("step: %f\n\r", float(i)/steps);
        }
        for(int i = steps; i > 0; i--) {
            moveMotorTo(&M1, &E1, &Encoder, float(i)/steps);
        }
    }
}


//function to mave a motor to a certain number of rotations, counted from the start of the program.
//parameters:
//DigitalOut *M = pointer to direction cpntol pin of motor
//PwmOut *E = pointer to speed contorl pin of motor
//QEI *Enc = pointer to encoder of motor
//float rotDes = desired rotation
void moveMotorTo(DigitalOut *M, PwmOut *E, QEI *Enc, float rotDes)
{
    float pErrorC;
    float pErrorP = 0;
    float iError = 0;
    float dError;

    float Kp = 5;
    float Ki = 0.01;
    float Kd = 0.7;

    float rotC = Enc->getPulses()/(32*131.25);
    float rotP = 0;
    float MotorPWM;

    Timer t;
    float tieme = 0;

    t.start();
    do {
        pErrorP = pErrorC;
        pErrorC = rotDes - rotC;
        iError = iError + pErrorC*tieme;
        dError = (pErrorC - pErrorP)/tieme;

        MotorPWM = pErrorC*Kp + iError*Ki + dError*Kd;

        if(MotorPWM > 0) {
            *M = 0;
            *E = MotorPWM;
        } else {
            *M = 1;
            *E = -MotorPWM;
        }

        rotP = rotC;
        rotC = Enc->getPulses()/(32*131.25);
        tieme = t.read();
        t.reset();
    } while (pErrorC > 0.001 || pErrorC < -0.001 ||dError > 0.01 || dError < -0.01);
    t.stop();
}


//double that calculates the rotation of one arm.
//parameters:
//double xDes = ofset of the arm's shaft in cm in the x direction
//double yDes = ofset of the arm's shaft in cm in the y direction
double calcRot1(double xDes, double yDes)
{
    return 6*((atan(yDes/xdes) - 0.5*(PI - acos((pow(xDes, 2) + pow(yDes, 2) - 2*pow(20, 2))/(-2*pow(20, 2)))))/(2*PI));
};

//double that calculates the rotation of the other arm.
//parameters:
//double xDes = ofset of the arm's shaft in cm in the x direction
//double yDes = ofset of the arm's shaft in cm in the y direction
double calcRot2(double xDes, double yDes)
{
    return 6*((atan(yDes/xdes) + 0.5*(PI - acos((pow(xDes, 2) + pow(yDes, 2) - 2*pow(20, 2))/(-2*pow(20, 2)))))/(2*PI));
};