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@0:441289ea4e29, 2021-05-27 (annotated)

Committer:
krogedal
Date:
Thu May 27 18:36:23 2021 +0000
Revision:
0:441289ea4e29
Child:
1:7c355adbc977
Set up basic motor control

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 0:441289ea4e29 20 double error = getError();
krogedal 0:441289ea4e29 21 double action = Kp * error + Ki * acc_err; //computes current error from encoder sample and the resulting action
krogedal 0:441289ea4e29 22 output += action;
krogedal 0:441289ea4e29 23 if(output > 1.0) { //limit checks and error flag set
krogedal 0:441289ea4e29 24 output = 1.0;
krogedal 0:441289ea4e29 25 max_out = 1; //this flag says not to increase speed more
krogedal 0:441289ea4e29 26 }
krogedal 0:441289ea4e29 27 else if(output < -1.0) {
krogedal 0:441289ea4e29 28 output = -1.0;
krogedal 0:441289ea4e29 29 max_out = 1;
krogedal 0:441289ea4e29 30 }
krogedal 0:441289ea4e29 31 else
krogedal 0:441289ea4e29 32 max_out = 0;
krogedal 0:441289ea4e29 33 mot->setOut(output); //set new output
krogedal 0:441289ea4e29 34 // pc.printf("output set to %2.2f\n", output);
krogedal 0:441289ea4e29 35 acc_err += error;
krogedal 0:441289ea4e29 36 }
krogedal 0:441289ea4e29 37
krogedal 0:441289ea4e29 38 MotorControl::MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ki, double ec)
krogedal 0:441289ea4e29 39 : encoder(a, b, i, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ki(ki) {
krogedal 0:441289ea4e29 40 output = 0.1; dir = 1; //default to avoid bugs
krogedal 0:441289ea4e29 41 max_out = 0; //flag set
krogedal 0:441289ea4e29 42 acc_err = 0;
krogedal 0:441289ea4e29 43 }
krogedal 0:441289ea4e29 44
krogedal 0:441289ea4e29 45 void MotorControl::setSpeed(double s) { //set speed in m/s. remember to consider the motor max speed
krogedal 0:441289ea4e29 46 r_speed = s;
krogedal 0:441289ea4e29 47 r_clicks = s / enc_const;
krogedal 0:441289ea4e29 48 // pc.printf("r_clicks set to %2.2f\n", r_clicks);
krogedal 0:441289ea4e29 49 output = s/max_speed;
krogedal 0:441289ea4e29 50 mot->setOut(output);
krogedal 0:441289ea4e29 51 }
krogedal 0:441289ea4e29 52
krogedal 0:441289ea4e29 53 void MotorControl::drive(void) {
krogedal 0:441289ea4e29 54 mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
krogedal 0:441289ea4e29 55 mot->drive(); //pc.printf("motor driving\n");
krogedal 0:441289ea4e29 56 //pc.printf("setting sampler at period %2.2f\n", period);
krogedal 0:441289ea4e29 57 sampler.attach(callback(this, &MotorControl::algorithm), period); //pc.printf("sampler set\n");
krogedal 0:441289ea4e29 58 }
krogedal 0:441289ea4e29 59
krogedal 0:441289ea4e29 60 void MotorControl::stop(void) {
krogedal 0:441289ea4e29 61 mot->stop();
krogedal 0:441289ea4e29 62 sampler.detach();
krogedal 0:441289ea4e29 63 acc_err = 0;
krogedal 0:441289ea4e29 64 }
krogedal 0:441289ea4e29 65
krogedal 0:441289ea4e29 66 void MotorControl::driveManual(void) {
krogedal 0:441289ea4e29 67 mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
krogedal 0:441289ea4e29 68 mot->drive(); //pc.printf("motor driving\n");
krogedal 0:441289ea4e29 69 }
krogedal 0:441289ea4e29 70
krogedal 0:441289ea4e29 71 void MotorControl::samplecall(void) {algorithm();}
krogedal 0:441289ea4e29 72
krogedal 0:441289ea4e29 73 void MotorControl::setK(double k) {Kp = k;}
krogedal 0:441289ea4e29 74
krogedal 0:441289ea4e29 75 void MotorControl::start(void) {} //this function is overridden to avoid bugs
krogedal 0:441289ea4e29 76 // double MotorControl::getError(void) {return (speed - getSpeed());}
krogedal 0:441289ea4e29 77 double MotorControl::getOutput(void) {return output;}
krogedal 0:441289ea4e29 78 bool MotorControl::checkFlag(void) {return max_out;}