KRAI ITB GARUDAGO / Mbed 2 deprecated frictionwheel

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

main.cpp@2:8619b626ef7d, 2021-12-10 (annotated)

Committer:
cnapitupulu19
Date:
Fri Dec 10 04:03:14 2021 +0000
Revision:
2:8619b626ef7d
Parent:
1:0b6d558a5431
Child:
3:aef2dcb63762
Kode terakhir buat friction wheel, belum ada PID hanya PWM;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
putriliza 0:62ded4362bbc 1 #include "mbed.h"
putriliza 0:62ded4362bbc 2 #include "encoderKRAI.h"
putriliza 0:62ded4362bbc 3 #include "Motor.h"
putriliza 0:62ded4362bbc 4 #include "pidLo.h"
putriliza 0:62ded4362bbc 5
putriliza 0:62ded4362bbc 6 #define PI 3.14159265358979f
putriliza 0:62ded4362bbc 7
cnapitupulu19 2:8619b626ef7d 8 #define MOTOR2_FORWARD PA_13
cnapitupulu19 2:8619b626ef7d 9 #define MOTOR2_REVERSE PA_14
cnapitupulu19 2:8619b626ef7d 10 #define MOTOR2_PWM PA_15
cnapitupulu19 2:8619b626ef7d 11
cnapitupulu19 2:8619b626ef7d 12 #define MOTOR1_FORWARD PC_0
cnapitupulu19 2:8619b626ef7d 13 #define MOTOR1_REVERSE PC_1
putriliza 0:62ded4362bbc 14 #define MOTOR1_PWM PB_0
putriliza 0:62ded4362bbc 15
kelvinsutirta 1:0b6d558a5431 16 #define PULSE_ENCODER 84
kelvinsutirta 1:0b6d558a5431 17
putriliza 0:62ded4362bbc 18 #define ENCODER1_CHANNEL_A PC_14
putriliza 0:62ded4362bbc 19 #define ENCODER1_CHANNEL_B PC_2
kelvinsutirta 1:0b6d558a5431 20 #define ENCODER2_CHANNEL_A PC_11
kelvinsutirta 1:0b6d558a5431 21 #define ENCODER2_CHANNEL_B PC_12
kelvinsutirta 1:0b6d558a5431 22
kelvinsutirta 1:0b6d558a5431 23 #define ENC_MOTOR_SAMP_US 20000
kelvinsutirta 1:0b6d558a5431 24 #define MOTOR_SAMP_US 5173
kelvinsutirta 1:0b6d558a5431 25 #define PID_SAMP_US 4173
kelvinsutirta 1:0b6d558a5431 26 #define S_TO_US 1000000
putriliza 0:62ded4362bbc 27
kelvinsutirta 1:0b6d558a5431 28 #define a_kp 0.0
kelvinsutirta 1:0b6d558a5431 29 #define a_ki 0.0
kelvinsutirta 1:0b6d558a5431 30 #define a_kd 0.0
kelvinsutirta 1:0b6d558a5431 31 #define a_TS 0.007
kelvinsutirta 1:0b6d558a5431 32
kelvinsutirta 1:0b6d558a5431 33 #define b_kp 0.0
kelvinsutirta 1:0b6d558a5431 34 #define b_ki 0.0
kelvinsutirta 1:0b6d558a5431 35 #define b_kd 0.0
kelvinsutirta 1:0b6d558a5431 36 #define b_TS 0.007
putriliza 0:62ded4362bbc 37
putriliza 0:62ded4362bbc 38 #define WHEEL_RAD 0.069 //meter
putriliza 0:62ded4362bbc 39
putriliza 0:62ded4362bbc 40 Motor motor1 (MOTOR1_PWM, MOTOR1_FORWARD, MOTOR1_REVERSE);
putriliza 0:62ded4362bbc 41 Motor motor2 (MOTOR2_PWM, MOTOR2_FORWARD, MOTOR2_REVERSE);
putriliza 0:62ded4362bbc 42
cnapitupulu19 2:8619b626ef7d 43 encoderKRAI encoder2 (ENCODER1_CHANNEL_A, ENCODER1_CHANNEL_B, PULSE_ENCODER, encoderKRAI::X4_ENCODING);
cnapitupulu19 2:8619b626ef7d 44 encoderKRAI encoder1 (ENCODER2_CHANNEL_A, ENCODER2_CHANNEL_B, PULSE_ENCODER, encoderKRAI::X4_ENCODING);
putriliza 0:62ded4362bbc 45
kelvinsutirta 1:0b6d558a5431 46 pidLo pid_motor1(a_kp, a_ki, 0, a_TS, 1, 0, 1000, 100);
kelvinsutirta 1:0b6d558a5431 47 pidLo pid_motor2(b_kp, b_ki, 0, b_TS, 1, 0, 1000, 100);
kelvinsutirta 1:0b6d558a5431 48
putriliza 0:62ded4362bbc 49 Serial pcUwu (USBTX, USBRX, 115200);
putriliza 0:62ded4362bbc 50
putriliza 0:62ded4362bbc 51 int pulse1 = 0;
putriliza 0:62ded4362bbc 52 int pulse2 = 0;
putriliza 0:62ded4362bbc 53 float v1_curr = 0;
putriliza 0:62ded4362bbc 54 float v2_curr = 0;
kelvinsutirta 1:0b6d558a5431 55 float a_pwm;
kelvinsutirta 1:0b6d558a5431 56 float b_pwm;
putriliza 0:62ded4362bbc 57
putriliza 0:62ded4362bbc 58 uint32_t sampling_time = 0;
putriliza 0:62ded4362bbc 59 uint32_t sampling_debug = 0;
putriliza 0:62ded4362bbc 60
cnapitupulu19 2:8619b626ef7d 61 float pengali = 1.0;
cnapitupulu19 2:8619b626ef7d 62
putriliza 0:62ded4362bbc 63 int main() {
putriliza 0:62ded4362bbc 64 while(1) {
cnapitupulu19 2:8619b626ef7d 65 motor1.speed(pengali*1.00);
cnapitupulu19 2:8619b626ef7d 66 motor2.speed(pengali*0.980);
kelvinsutirta 1:0b6d558a5431 67 if (us_ticker_read()-sampling_time >= ENC_MOTOR_SAMP_US) {
putriliza 0:62ded4362bbc 68 pulse1 = encoder1.getPulses();
putriliza 0:62ded4362bbc 69 pulse2 = encoder2.getPulses();
putriliza 0:62ded4362bbc 70 v1_curr = (float) pulse1*2*PI*WHEEL_RAD*S_TO_US/(PULSE_ENCODER*ENC_MOTOR_SAMP_US);
putriliza 0:62ded4362bbc 71 v2_curr = (float) pulse2*2*PI*WHEEL_RAD*S_TO_US/(PULSE_ENCODER*ENC_MOTOR_SAMP_US);
putriliza 0:62ded4362bbc 72
putriliza 0:62ded4362bbc 73 /* reset nilai encoder */
putriliza 0:62ded4362bbc 74 encoder1.reset();
putriliza 0:62ded4362bbc 75 encoder2.reset();
putriliza 0:62ded4362bbc 76 sampling_time = us_ticker_read();
putriliza 0:62ded4362bbc 77 }
putriliza 0:62ded4362bbc 78
putriliza 0:62ded4362bbc 79 if (us_ticker_read()-sampling_debug >= 100000) {
kelvinsutirta 1:0b6d558a5431 80 pcUwu.printf("%.3f, %.3f\n", v1_curr, v2_curr);
putriliza 0:62ded4362bbc 81 sampling_debug = us_ticker_read();
putriliza 0:62ded4362bbc 82 }
kelvinsutirta 1:0b6d558a5431 83
cnapitupulu19 2:8619b626ef7d 84 // if(us_ticker_read()-sampling_debug >= PID_SAMP_US){
cnapitupulu19 2:8619b626ef7d 85 // // a_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
cnapitupulu19 2:8619b626ef7d 86 //// b_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
cnapitupulu19 2:8619b626ef7d 87 // }
cnapitupulu19 2:8619b626ef7d 88 //
cnapitupulu19 2:8619b626ef7d 89 // if(us_ticker_read()-sampling_debug >= MOTOR_SAMP_US){
cnapitupulu19 2:8619b626ef7d 90 //// motor.speeda_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
cnapitupulu19 2:8619b626ef7d 91 //// b_pwm = pid_motor1.createpwm(v1_target, v1_curr, 1.0);
cnapitupulu19 2:8619b626ef7d 92 // }
putriliza 0:62ded4362bbc 93 }
putriliza 0:62ded4362bbc 94 }