MotorControl.cpp

00001 /* Karbot motor control class
00002  * Written by Simon Krogedal
00003  * 27/05/21
00004  * Team 9 4th Year project
00005  * 
00006  * for NUCLEO-F401RE
00007  * 
00008  */
00009  
00010  #include "motor.h"
00011  #include "MotorControl.h"
00012  
00013  
00014 double MotorControl::getError(void) {return (r_clicks - getClicks());}
00015 
00016 void MotorControl::algorithm(void) {
00017 //          pc.printf("speed adjusting\n");
00018     sample_func();
00019     double error = getError();               
00020     output += Kp * (error + Ti*error - prev_error);             //computes current error from encoder sample and the resulting action
00021     if(output > 1.0) {                                          //limit checks and error flag set
00022         output = 1.0;
00023         max_out = 1;                                            //this flag says not to increase speed more
00024     }
00025     else if(output < -1.0) {
00026         output = -1.0;
00027         max_out = 1;
00028     }
00029     else
00030         max_out = 0;
00031     mot->setOut(output);                                        //set new output
00032 //          pc.printf("output set to %2.2f\n", output);
00033     prev_error = error;
00034 }
00035 
00036 MotorControl::MotorControl(PinName EncoderChanA, PinName EncoderChanB, int CPR, encoder::Side side, double period, motor* Motor, double MaxSpeed, double kp, double ti, double diameter)
00037             : encoder(EncoderChanA, EncoderChanB, CPR, side, period, diameter), mot(Motor), max_speed(MaxSpeed), Kp(kp), Ti(ti) {
00038     output = 0.1; dir = 1;                          //default to avoid bugs
00039     max_out = 0;                                    //flag set
00040     prev_error = 0;
00041 }
00042 
00043 void MotorControl::setSpeed(double speed) { //set speed in m/s. remember to consider the motor max speed
00044     r_speed = speed;
00045     r_clicks = speed / enc_const;
00046 //          pc.printf("r_clicks set to %2.2f\n", r_clicks);
00047 }
00048 
00049 void MotorControl::drive(void) {
00050     mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
00051     mot->drive(); //pc.printf("motor driving\n");
00052     //pc.printf("setting sampler at period %2.2f\n", period);
00053     sampler.attach(callback(this, &MotorControl::algorithm), period); //pc.printf("sampler set\n");
00054 }
00055 
00056 void MotorControl::stop(void) {
00057     mot->stop();
00058     sampler.detach();
00059     prev_error = 0;
00060 }
00061 
00062 void MotorControl::driveManual(void) {
00063     mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
00064     mot->drive(); //pc.printf("motor driving\n");
00065 }
00066 
00067 void MotorControl::samplecall(void) {algorithm();}
00068 
00069 void MotorControl::setK(double k) {Kp = k;}
00070 
00071 //void MotorControl::start(void) {} //this function is overridden to avoid bugs
00072 //      double MotorControl::getError(void) {return (speed - getSpeed());}
00073 double MotorControl::getOutput(void) {return output;}
00074 bool MotorControl::checkFlag(void) {return max_out;}