Simon Krogedal / Karbot_wheel_control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed ros_lib_melodic

src/main.cpp@2:aeaa1c1e0d33, 2021-05-27 (annotated)

Committer:
krogedal
Date:
Thu May 27 19:26:21 2021 +0000
Revision:
2:aeaa1c1e0d33
Parent:
1:7c355adbc977
Child:
3:4b6080e86761
moved back to mbed 2.0, as this is required for ros melodic libs to work

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krogedal 0:441289ea4e29 1 /* Karbot motor controller
krogedal 0:441289ea4e29 2 * Written by Simon Krogedal
krogedal 0:441289ea4e29 3 * 26/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 //LIBRARIES
krogedal 0:441289ea4e29 11 #include "mbed.h"
krogedal 0:441289ea4e29 12
krogedal 0:441289ea4e29 13 #include "motor.h"
krogedal 0:441289ea4e29 14 #include "encoder.h"
krogedal 0:441289ea4e29 15 #include "MotorControl.h"
krogedal 0:441289ea4e29 16
krogedal 0:441289ea4e29 17 //PIN DEFINITIONS
krogedal 0:441289ea4e29 18 #define L_MOTOR_PIN PC_6
krogedal 0:441289ea4e29 19 #define R_MOTOR_PIN PB_14
krogedal 0:441289ea4e29 20 #define L_MOTOR_DIR PC_15
krogedal 0:441289ea4e29 21 #define R_MOTOR_DIR PC_14
krogedal 0:441289ea4e29 22 #define L_ENC_PIN_A PA_14
krogedal 0:441289ea4e29 23 #define L_ENC_PIN_B PB_15
krogedal 0:441289ea4e29 24 #define R_ENC_PIN_A PA_13
krogedal 0:441289ea4e29 25 #define R_ENC_PIN_B PB_1
krogedal 0:441289ea4e29 26
krogedal 0:441289ea4e29 27
krogedal 0:441289ea4e29 28 //VARIABLES
krogedal 0:441289ea4e29 29 #define MOTOR_FREQ 25000.0 //25 kHz
krogedal 0:441289ea4e29 30 #define MOTOR_TS 0.05 //sample time, ie the time between every PID calculation
krogedal 0:441289ea4e29 31 #define MOTOR_KP 0.00002
krogedal 0:441289ea4e29 32 #define MOTOR_KI 0.0000001
krogedal 0:441289ea4e29 33
krogedal 0:441289ea4e29 34
krogedal 0:441289ea4e29 35 //CONSTANTS
krogedal 0:441289ea4e29 36 #define WHEEL_RAD 0.1025 //radius in meters
krogedal 0:441289ea4e29 37 #define PI 3.1415
krogedal 0:441289ea4e29 38 #define ENCODER_PERIOD 0.05 //encoder sampling period in seconds (currently 1/20 sec)
krogedal 0:441289ea4e29 39 #define ENCODER_CPR 980 //clicks per revolution
krogedal 2:aeaa1c1e0d33 40 #define L_MAX_SPEED 1.5
krogedal 2:aeaa1c1e0d33 41 #define R_MAX_SPEED 1.5
krogedal 0:441289ea4e29 42
krogedal 0:441289ea4e29 43 /* Functionality Summary
krogedal 0:441289ea4e29 44
krogedal 0:441289ea4e29 45 */
krogedal 0:441289ea4e29 46
krogedal 0:441289ea4e29 47 int main() {
krogedal 0:441289ea4e29 48
krogedal 0:441289ea4e29 49 // Constant used by QEI libraries
krogedal 0:441289ea4e29 50 double enc_const = ((WHEEL_RAD * 2.0 * PI) / ((double)(4 * ENCODER_CPR)));
krogedal 0:441289ea4e29 51 // Motor PWM frequency
krogedal 0:441289ea4e29 52 double motT = 1/MOTOR_FREQ;
krogedal 0:441289ea4e29 53
krogedal 0:441289ea4e29 54 // Initialisation
krogedal 0:441289ea4e29 55 motor leftMot(L_MOTOR_PIN, L_MOTOR_DIR, motT);
krogedal 0:441289ea4e29 56 //motor rightMot(R_MOTOR_PIN, R_MOTOR_DIR, motT);
krogedal 0:441289ea4e29 57
krogedal 0:441289ea4e29 58 MotorControl leftCtrl(L_ENC_PIN_A, L_ENC_PIN_B, NC, ENCODER_CPR, 1, MOTOR_TS, &leftMot, L_MAX_SPEED, MOTOR_KP, MOTOR_KI, enc_const);
krogedal 0:441289ea4e29 59 //MotorControl rightCtrl(R_ENC_PIN_A, R_ENC_PIN_B, NC, ENCODER_CPR, 0, MOTOR_TS, &rightMot, R_MAX_SPEED, MOTOR_KP, MOTOR_KI, enc_const);
krogedal 0:441289ea4e29 60
krogedal 0:441289ea4e29 61
krogedal 0:441289ea4e29 62 leftCtrl.setSpeed(0.5);
krogedal 0:441289ea4e29 63 leftCtrl.driveManual();
krogedal 0:441289ea4e29 64
krogedal 0:441289ea4e29 65 while (true) {
krogedal 0:441289ea4e29 66 leftCtrl.setSpeed(0.5);
krogedal 2:aeaa1c1e0d33 67 //ThisThread::sleep_for(3s);
krogedal 0:441289ea4e29 68 leftCtrl.setSpeed(1.2);
krogedal 2:aeaa1c1e0d33 69 //ThisThread::sleep_for(3s);
krogedal 0:441289ea4e29 70 }
krogedal 0:441289ea4e29 71 }