File content as of revision 3:4b6080e86761:

/* Karbot motor control class
 * Written by Simon Krogedal
 * 27/05/21
 * Team 9 4th Year project
 * 
 * for NUCLEO-F401RE
 * 
 */
 
 #include "mbed.h"
 #include "motor.h"
 #include "MotorControl.h"
 
 
double MotorControl::getError(void) {return (r_clicks - getClicks());}

void MotorControl::algorithm(void) {
//          pc.printf("speed adjusting\n");
    sample_func();
    double error = getError();               
    output += Kp * (error + Ti*error - prev_error);             //computes current error from encoder sample and the resulting action
    if(output > 1.0) {                                          //limit checks and error flag set
        output = 1.0;
        max_out = 1;                                            //this flag says not to increase speed more
    }
    else if(output < -1.0) {
        output = -1.0;
        max_out = 1;
    }
    else
        max_out = 0;
    mot->setOut(output);                                        //set new output
//          pc.printf("output set to %2.2f\n", output);
    prev_error = error;
}

MotorControl::MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ti, double ec)
            : encoder(a, b, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ti(ti) {
    output = 0.1; dir = 1;                          //default to avoid bugs
    max_out = 0;                                    //flag set
    prev_error = 0;
}

void MotorControl::setSpeed(double s) { //set speed in m/s. remember to consider the motor max speed
    r_speed = s;
    r_clicks = s / enc_const;
//          pc.printf("r_clicks set to %2.2f\n", r_clicks);
}

void MotorControl::drive(void) {
    mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
    mot->drive(); //pc.printf("motor driving\n");
    //pc.printf("setting sampler at period %2.2f\n", period);
    sampler.attach(callback(this, &MotorControl::algorithm), period); //pc.printf("sampler set\n");
}

void MotorControl::stop(void) {
    mot->stop();
    sampler.detach();
    prev_error = 0;
}

void MotorControl::driveManual(void) {
    mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
    mot->drive(); //pc.printf("motor driving\n");
}

void MotorControl::samplecall(void) {algorithm();}

void MotorControl::setK(double k) {Kp = k;}

void MotorControl::start(void) {} //this function is overridden to avoid bugs
//      double MotorControl::getError(void) {return (speed - getSpeed());}
double MotorControl::getOutput(void) {return output;}
bool MotorControl::checkFlag(void) {return max_out;}