Simon Krogedal / Lift_arm_control

Control project for the Lift-arm. Works with ROS Melodic

Dependencies:   mbed Servo ros_lib_melodic ULN2003_StepperDriver Async_4pin_Stepper

src/MotorControl.cpp@4:9edb248c6431, 2021-05-31 (annotated)

Committer:
krogedal
Date:
Mon May 31 16:47:02 2021 +0000
Revision:
4:9edb248c6431
Parent:
3:4b6080e86761 
Updated to work with ROS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krogedal 0:441289ea4e29 1 /* Karbot motor control class
krogedal 0:441289ea4e29 2 * Written by Simon Krogedal
krogedal 0:441289ea4e29 3 * 27/05/21
krogedal 0:441289ea4e29 4 * Team 9 4th Year project
krogedal 0:441289ea4e29 5 *
krogedal 0:441289ea4e29 6 * for NUCLEO-F401RE
krogedal 0:441289ea4e29 7 *
krogedal 0:441289ea4e29 8 */
krogedal 0:441289ea4e29 9
krogedal 0:441289ea4e29 10 #include "mbed.h"
krogedal 0:441289ea4e29 11 #include "motor.h"
krogedal 0:441289ea4e29 12 #include "MotorControl.h"
krogedal 0:441289ea4e29 13
krogedal 0:441289ea4e29 14
krogedal 0:441289ea4e29 15 double MotorControl::getError(void) {return (r_clicks - getClicks());}
krogedal 0:441289ea4e29 16
krogedal 0:441289ea4e29 17 void MotorControl::algorithm(void) {
krogedal 0:441289ea4e29 18 // pc.printf("speed adjusting\n");
krogedal 0:441289ea4e29 19 sample_func();
krogedal 3:4b6080e86761 20 double error = getError();
krogedal 3:4b6080e86761 21 output += Kp * (error + Ti*error - prev_error); //computes current error from encoder sample and the resulting action
krogedal 0:441289ea4e29 22 if(output > 1.0) { //limit checks and error flag set
krogedal 0:441289ea4e29 23 output = 1.0;
krogedal 0:441289ea4e29 24 max_out = 1; //this flag says not to increase speed more
krogedal 0:441289ea4e29 25 }
krogedal 0:441289ea4e29 26 else if(output < -1.0) {
krogedal 0:441289ea4e29 27 output = -1.0;
krogedal 0:441289ea4e29 28 max_out = 1;
krogedal 0:441289ea4e29 29 }
krogedal 0:441289ea4e29 30 else
krogedal 0:441289ea4e29 31 max_out = 0;
krogedal 0:441289ea4e29 32 mot->setOut(output); //set new output
krogedal 0:441289ea4e29 33 // pc.printf("output set to %2.2f\n", output);
krogedal 3:4b6080e86761 34 prev_error = error;
krogedal 0:441289ea4e29 35 }
krogedal 0:441289ea4e29 36
krogedal 3:4b6080e86761 37 MotorControl::MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ti, double ec)
krogedal 3:4b6080e86761 38 : encoder(a, b, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ti(ti) {
krogedal 0:441289ea4e29 39 output = 0.1; dir = 1; //default to avoid bugs
krogedal 0:441289ea4e29 40 max_out = 0; //flag set
krogedal 3:4b6080e86761 41 prev_error = 0;
krogedal 0:441289ea4e29 42 }
krogedal 0:441289ea4e29 43
krogedal 0:441289ea4e29 44 void MotorControl::setSpeed(double s) { //set speed in m/s. remember to consider the motor max speed
krogedal 0:441289ea4e29 45 r_speed = s;
krogedal 0:441289ea4e29 46 r_clicks = s / enc_const;
krogedal 0:441289ea4e29 47 // pc.printf("r_clicks set to %2.2f\n", r_clicks);
krogedal 0:441289ea4e29 48 }
krogedal 0:441289ea4e29 49
krogedal 0:441289ea4e29 50 void MotorControl::drive(void) {
krogedal 0:441289ea4e29 51 mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
krogedal 0:441289ea4e29 52 mot->drive(); //pc.printf("motor driving\n");
krogedal 0:441289ea4e29 53 //pc.printf("setting sampler at period %2.2f\n", period);
krogedal 0:441289ea4e29 54 sampler.attach(callback(this, &MotorControl::algorithm), period); //pc.printf("sampler set\n");
krogedal 0:441289ea4e29 55 }
krogedal 0:441289ea4e29 56
krogedal 0:441289ea4e29 57 void MotorControl::stop(void) {
krogedal 0:441289ea4e29 58 mot->stop();
krogedal 0:441289ea4e29 59 sampler.detach();
krogedal 3:4b6080e86761 60 prev_error = 0;
krogedal 0:441289ea4e29 61 }
krogedal 0:441289ea4e29 62
krogedal 0:441289ea4e29 63 void MotorControl::driveManual(void) {
krogedal 0:441289ea4e29 64 mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
krogedal 0:441289ea4e29 65 mot->drive(); //pc.printf("motor driving\n");
krogedal 0:441289ea4e29 66 }
krogedal 0:441289ea4e29 67
krogedal 0:441289ea4e29 68 void MotorControl::samplecall(void) {algorithm();}
krogedal 0:441289ea4e29 69
krogedal 0:441289ea4e29 70 void MotorControl::setK(double k) {Kp = k;}
krogedal 0:441289ea4e29 71
krogedal 0:441289ea4e29 72 void MotorControl::start(void) {} //this function is overridden to avoid bugs
krogedal 0:441289ea4e29 73 // double MotorControl::getError(void) {return (speed - getSpeed());}
krogedal 0:441289ea4e29 74 double MotorControl::getOutput(void) {return output;}
krogedal 0:441289ea4e29 75 bool MotorControl::checkFlag(void) {return max_out;}