Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@20:ac1b4ffa3323, 2019-10-04 (annotated)

Committer:
Hendrikvg
Date:
Fri Oct 04 13:28:00 2019 +0000
Revision:
20:ac1b4ffa3323
Parent:
19:5ac8b7af77a3
Child:
21:394a7a1deb73
torque_p van de controller werkt, verder werkt er nog niets.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hendrikvg 17:cacf9e75eda7 1 #include "QEI.h"
Hendrikvg 15:80b3ac2b8448 2 #include "mbed.h"
Hendrikvg 20:ac1b4ffa3323 3 #include "BiQuad.h"
Hendrikvg 14:20f11bb58244 4 #include "FastPWM.h"
Hendrikvg 17:cacf9e75eda7 5 #include "HIDScope.h"
Hendrikvg 16:40183eeadb6d 6 #include "MODSERIAL.h"
Hendrikvg 9:12b9865e7373 7
Hendrikvg 18:50c04dd523ea 8 // ENC1A --> D13
Hendrikvg 18:50c04dd523ea 9 // ENC1B --> D12
Hendrikvg 18:50c04dd523ea 10 // POT1 --> A0
Hendrikvg 18:50c04dd523ea 11 // LED1 --> D3
Hendrikvg 18:50c04dd523ea 12 // BUT1 --> D1
Hendrikvg 18:50c04dd523ea 13 // BUT2 --> D0
Hendrikvg 18:50c04dd523ea 14
Hendrikvg 20:ac1b4ffa3323 15 MODSERIAL pc(USBTX, USBRX);
Hendrikvg 20:ac1b4ffa3323 16 Ticker ControlMotor1;
Hendrikvg 15:80b3ac2b8448 17
Hendrikvg 20:ac1b4ffa3323 18 AnalogIn theta_ref(A0);
Hendrikvg 20:ac1b4ffa3323 19 DigitalOut PWM_motor_1(D6);
Hendrikvg 20:ac1b4ffa3323 20 DigitalOut direction_motor_1(D7);
RobertoO 0:67c50348f842 21
Hendrikvg 17:cacf9e75eda7 22 HIDScope scope(2);
Hendrikvg 17:cacf9e75eda7 23 QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING);
Hendrikvg 17:cacf9e75eda7 24 Ticker RW_scope;
Hendrikvg 17:cacf9e75eda7 25
Hendrikvg 15:80b3ac2b8448 26 // variables
Hendrikvg 20:ac1b4ffa3323 27 float duty_cycle_motor_1;
Hendrikvg 16:40183eeadb6d 28 volatile int on_time_ms; // The time the LED should be on, in microseconds
Hendrikvg 16:40183eeadb6d 29 volatile int off_time_ms;
Hendrikvg 15:80b3ac2b8448 30
Hendrikvg 20:ac1b4ffa3323 31 float theta = 0;
Hendrikvg 20:ac1b4ffa3323 32 volatile double x_0;
Hendrikvg 20:ac1b4ffa3323 33 volatile double x_prev_0 = 0;
Hendrikvg 20:ac1b4ffa3323 34 volatile double y_0;
Hendrikvg 20:ac1b4ffa3323 35 volatile double x_1;
Hendrikvg 20:ac1b4ffa3323 36 volatile double x_prev_1 = 0;
Hendrikvg 20:ac1b4ffa3323 37 volatile double y_1;
Hendrikvg 20:ac1b4ffa3323 38
Hendrikvg 20:ac1b4ffa3323 39 float omega;
Hendrikvg 20:ac1b4ffa3323 40 float torque;
Hendrikvg 20:ac1b4ffa3323 41 float torque_p;
Hendrikvg 20:ac1b4ffa3323 42 float torque_i;
Hendrikvg 20:ac1b4ffa3323 43 float torque_d;
Hendrikvg 20:ac1b4ffa3323 44 float velocity;
Hendrikvg 20:ac1b4ffa3323 45 float theta_error;
Hendrikvg 20:ac1b4ffa3323 46
Hendrikvg 20:ac1b4ffa3323 47 float Kp = 5;
Hendrikvg 20:ac1b4ffa3323 48 float Ki = 3;
Hendrikvg 20:ac1b4ffa3323 49 float Kd = 3;
Hendrikvg 20:ac1b4ffa3323 50 float Ts = 1;
Hendrikvg 17:cacf9e75eda7 51
Hendrikvg 15:80b3ac2b8448 52 // functions
Hendrikvg 20:ac1b4ffa3323 53 float CalculateError(){
Hendrikvg 20:ac1b4ffa3323 54 //theta_error = (360*theta_ref.read())-theta;
Hendrikvg 20:ac1b4ffa3323 55 theta_error = 180;
Hendrikvg 20:ac1b4ffa3323 56 return theta_error;}
Hendrikvg 16:40183eeadb6d 57
Hendrikvg 20:ac1b4ffa3323 58 float Controller(float theta_error){
Hendrikvg 20:ac1b4ffa3323 59 //static float error_integral = 0;
Hendrikvg 20:ac1b4ffa3323 60 //static float error_prev = 0;
Hendrikvg 20:ac1b4ffa3323 61 //static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
Hendrikvg 20:ac1b4ffa3323 62
Hendrikvg 20:ac1b4ffa3323 63 // Proportional part:
Hendrikvg 20:ac1b4ffa3323 64 float torque_p = Kp * theta_error;
Hendrikvg 20:ac1b4ffa3323 65
Hendrikvg 20:ac1b4ffa3323 66 // Integral part:
Hendrikvg 20:ac1b4ffa3323 67 //error_integral = error_integral + theta_error * Ts;
Hendrikvg 20:ac1b4ffa3323 68 //float torque_i = Ki * error_integral;
Hendrikvg 20:ac1b4ffa3323 69
Hendrikvg 20:ac1b4ffa3323 70 // Derivative part:
Hendrikvg 20:ac1b4ffa3323 71 //float error_derivative = (theta_error - error_prev)/Ts;
Hendrikvg 20:ac1b4ffa3323 72 //float filtered_error_derivative = LowPassFilter.step(error_derivative);
Hendrikvg 20:ac1b4ffa3323 73 //float torque_d = Kd * filtered_error_derivative;
Hendrikvg 20:ac1b4ffa3323 74 //float torque_d = Kd*error_derivative;
Hendrikvg 20:ac1b4ffa3323 75 //error_prev = theta_error;
Hendrikvg 20:ac1b4ffa3323 76
Hendrikvg 20:ac1b4ffa3323 77 // Sum all parts and return it
Hendrikvg 20:ac1b4ffa3323 78 //float torque = torque_p + torque_i + torque_d;
Hendrikvg 20:ac1b4ffa3323 79 return torque_p;}
Hendrikvg 16:40183eeadb6d 80
Hendrikvg 20:ac1b4ffa3323 81 void CalculateDirectionMotor1() {
Hendrikvg 20:ac1b4ffa3323 82 if (theta_error >= 0) {
Hendrikvg 20:ac1b4ffa3323 83 direction_motor_1 = 1;}
Hendrikvg 20:ac1b4ffa3323 84 else {
Hendrikvg 20:ac1b4ffa3323 85 direction_motor_1 = 0;}}
Hendrikvg 20:ac1b4ffa3323 86
Hendrikvg 20:ac1b4ffa3323 87 //float CalculateVelocityMotor1() {
Hendrikvg 20:ac1b4ffa3323 88 // return theta_error/Ts;}
Hendrikvg 16:40183eeadb6d 89
Hendrikvg 16:40183eeadb6d 90 void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM
Hendrikvg 20:ac1b4ffa3323 91 { on_time_ms = (int) (duty_cycle_motor_1*(1/1e2)*1e3); // Deze variable maken
Hendrikvg 20:ac1b4ffa3323 92 off_time_ms = (int) ((1-duty_cycle_motor_1)*(1/1e2)*1e3);
Hendrikvg 19:5ac8b7af77a3 93 PWM_motor_1 = 1;
Hendrikvg 16:40183eeadb6d 94 wait_ms(on_time_ms);
Hendrikvg 19:5ac8b7af77a3 95 PWM_motor_1 = 0;
Hendrikvg 19:5ac8b7af77a3 96 wait_ms(off_time_ms);}
Hendrikvg 20:ac1b4ffa3323 97
Hendrikvg 20:ac1b4ffa3323 98 void MotorDirection(){
Hendrikvg 20:ac1b4ffa3323 99 }
Hendrikvg 16:40183eeadb6d 100
Hendrikvg 19:5ac8b7af77a3 101 void ReadEncoderAndWriteScope(){
Hendrikvg 20:ac1b4ffa3323 102 theta = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
Hendrikvg 20:ac1b4ffa3323 103 x_0 = theta;
Hendrikvg 20:ac1b4ffa3323 104 y_0 = (x_prev_0 + x_0)/2.0;
Hendrikvg 20:ac1b4ffa3323 105 scope.set(0,y_0);
Hendrikvg 20:ac1b4ffa3323 106 x_prev_0 = x_0;
Hendrikvg 20:ac1b4ffa3323 107 y_1 = (x_0 - x_prev_0)/Ts;
Hendrikvg 20:ac1b4ffa3323 108 scope.set(1,y_1);
Hendrikvg 19:5ac8b7af77a3 109 scope.send();}
Hendrikvg 17:cacf9e75eda7 110
Hendrikvg 20:ac1b4ffa3323 111 //void doeietsnuttigs(){
Hendrikvg 20:ac1b4ffa3323 112 // Controller(CalculateError());}
Hendrikvg 20:ac1b4ffa3323 113
Hendrikvg 15:80b3ac2b8448 114 // main
Hendrikvg 15:80b3ac2b8448 115 int main()
Hendrikvg 15:80b3ac2b8448 116 {
RobertoO 0:67c50348f842 117 pc.baud(115200);
Hendrikvg 17:cacf9e75eda7 118 pc.printf("Hello World!\n\r");
Hendrikvg 20:ac1b4ffa3323 119 //ControlMotor1.attach(&ReadEncoderAndWriteScope, Ts);
Hendrikvg 20:ac1b4ffa3323 120 //ControlMotor1.attach(&doeietsnuttigs, Ts);
Hendrikvg 20:ac1b4ffa3323 121 while(1) {
Hendrikvg 20:ac1b4ffa3323 122 pc.printf("%f \n\r",Controller(CalculateError()));
Hendrikvg 20:ac1b4ffa3323 123 wait(1);
Hendrikvg 9:12b9865e7373 124 }
Hendrikvg 2:d9b0ebf3fcca 125 }