Rick Kouwen / Mbed 2 deprecated template_biorobotics

LED toggle

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@4:bb9bcbb22882, 2019-10-07 (annotated)

Committer:
rick309
Date:
Mon Oct 07 09:32:26 2019 +0000
Revision:
4:bb9bcbb22882
Parent:
3:390b7ac07433
Child:
5:532b9a72e012
Werkend PWM geregelde motor;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
rick309 4:bb9bcbb22882 2 #include "HIDScope.h"
rick309 3:390b7ac07433 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
rick309 4:bb9bcbb22882 5 #include "BiQuad.h"
rick309 3:390b7ac07433 6 #include "FastPWM.h"
rick309 3:390b7ac07433 7 #include "math.h"
rick309 4:bb9bcbb22882 8
rick309 4:bb9bcbb22882 9 MODSERIAL pc(USBTX, USBRX);
rick309 4:bb9bcbb22882 10 QEI motor1 (D8, D9, NC, 32);
rick309 4:bb9bcbb22882 11 FastPWM motor1_pwm(D5);
rick309 4:bb9bcbb22882 12 DigitalOut motor1_dir(D4);
rick309 4:bb9bcbb22882 13 AnalogIn pod1(A0);
rick309 4:bb9bcbb22882 14 AnalogIn pod2(A1);
rick309 4:bb9bcbb22882 15
RobertoO 0:67c50348f842 16
rick309 4:bb9bcbb22882 17 Ticker measure;
rick309 4:bb9bcbb22882 18
rick309 4:bb9bcbb22882 19 int m1_count;
rick309 4:bb9bcbb22882 20 float m1_angle;
rick309 4:bb9bcbb22882 21 float pi = 3.14;
rick309 4:bb9bcbb22882 22 float pod1_angle;
rick309 4:bb9bcbb22882 23 float pod2_angle;
rick309 4:bb9bcbb22882 24 float kp = 10;
rick309 4:bb9bcbb22882 25 float motor1_er;
rick309 4:bb9bcbb22882 26 float motor1_pwm_signal;
rick309 3:390b7ac07433 27
rick309 4:bb9bcbb22882 28 void getCounts(){
rick309 4:bb9bcbb22882 29 m1_count = motor1.getPulses();
rick309 4:bb9bcbb22882 30 m1_angle = (float)m1_count / 4200.0f * (2.0f * pi);
rick309 4:bb9bcbb22882 31 pod1_angle = (float)pod1 * 2.0f * pi;
rick309 4:bb9bcbb22882 32 pod2_angle = (float)pod2;
rick309 4:bb9bcbb22882 33 }
rick309 4:bb9bcbb22882 34
rick309 4:bb9bcbb22882 35 void error(void){
rick309 4:bb9bcbb22882 36 motor1_er = pod1_angle - m1_angle;
rick309 4:bb9bcbb22882 37 }
rick309 4:bb9bcbb22882 38 void pwm_motor1(){
rick309 4:bb9bcbb22882 39 motor1_pwm_signal = kp * fabs((motor1_er * pod2_angle));
rick309 3:390b7ac07433 40 }
rick309 4:bb9bcbb22882 41 void control(){
rick309 4:bb9bcbb22882 42 if(fabs(motor1_er) < 0.1f){
rick309 4:bb9bcbb22882 43 motor1_pwm.write(0);
rick309 4:bb9bcbb22882 44 }else if(motor1_er < 0.0f){
rick309 4:bb9bcbb22882 45 motor1_pwm.write(motor1_pwm_signal);
rick309 4:bb9bcbb22882 46 motor1_dir = 1;
rick309 4:bb9bcbb22882 47 }else if(motor1_er > 0.0f){
rick309 4:bb9bcbb22882 48 motor1_pwm.write(motor1_pwm_signal);
rick309 4:bb9bcbb22882 49 motor1_dir = 0;
rick309 4:bb9bcbb22882 50 }
rick309 3:390b7ac07433 51 }
rick309 4:bb9bcbb22882 52
rick309 4:bb9bcbb22882 53 void MeasureandControl(){
rick309 4:bb9bcbb22882 54 getCounts();
rick309 4:bb9bcbb22882 55 error();
rick309 4:bb9bcbb22882 56 pwm_motor1();
rick309 4:bb9bcbb22882 57 control();
rick309 3:390b7ac07433 58 }
RobertoO 0:67c50348f842 59
rick309 4:bb9bcbb22882 60 int main()
rick309 4:bb9bcbb22882 61 {
rick309 4:bb9bcbb22882 62 pc.baud(115200);
rick309 4:bb9bcbb22882 63 pc.printf("\r\nStarting up...\r\n\r\n");
rick309 4:bb9bcbb22882 64 motor1_pwm.period(1.0/10000);
rick309 4:bb9bcbb22882 65 motor1_dir = 0;
rick309 4:bb9bcbb22882 66 measure.attach(MeasureandControl,0.01);
rick309 4:bb9bcbb22882 67 while(true){
rick309 4:bb9bcbb22882 68 pc.printf("\r\nMotor angle: %f Pod1 angle: %f K_p: %f Error: %f",m1_angle,pod1_angle,motor1_pwm_signal,motor1_er);
rick309 4:bb9bcbb22882 69 wait(0.8);
rick309 3:390b7ac07433 70 }
rick309 4:bb9bcbb22882 71 }