Matthew Maat / Mbed 2 deprecated template_biorobotics

fancy lampje

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM

main.cpp@8:ec3c634390c7, 2019-09-20 (annotated)

Committer:
MatthewMaat
Date:
Fri Sep 20 09:20:00 2019 +0000
Revision:
8:ec3c634390c7
Parent:
7:a32766b96d91
Child:
9:ae548d05ec71
HID from slides

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
MatthewMaat 8:ec3c634390c7 2 #include "HIDScope.h"
MatthewMaat 8:ec3c634390c7 3 #include "QEI.h"
RobertoO 1:b862262a9d14 4 #include "MODSERIAL.h"
RobertoO 0:67c50348f842 5 //#include "BiQuad.h"
MatthewMaat 8:ec3c634390c7 6 #include "FastPWM.h"
MatthewMaat 2:626688c21b6f 7 #include <iostream>
MatthewMaat 5:cee5f898b350 8 MODSERIAL pc(USBTX, USBRX);
MatthewMaat 8:ec3c634390c7 9 Encoder motor1(D13,D12);
MatthewMaat 8:ec3c634390c7 10 PwmOut led(D9);
MatthewMaat 8:ec3c634390c7 11 HIDScope scope(3);
MatthewMaat 7:a32766b96d91 12
MatthewMaat 8:ec3c634390c7 13
MatthewMaat 8:ec3c634390c7 14
MatthewMaat 8:ec3c634390c7 15 int main()
MatthewMaat 4:f988679bf9a1 16 {
MatthewMaat 8:ec3c634390c7 17 const float degrees_per_count = 90/200.0; //quick approach
MatthewMaat 8:ec3c634390c7 18 const float ain_degrees_range = 90; //degrees over which the potmeter can control.
MatthewMaat 8:ec3c634390c7 19 const float kp_1_range = -1;
MatthewMaat 8:ec3c634390c7 20 AnalogIn setpoint_1(A0);
MatthewMaat 8:ec3c634390c7 21 AnalogIn p_value(A1);
MatthewMaat 8:ec3c634390c7 22 DigitalOut direction_1(D4);
MatthewMaat 8:ec3c634390c7 23 PwmOut pwm_1(D5);
MatthewMaat 8:ec3c634390c7 24 float degrees_setpoint;
MatthewMaat 8:ec3c634390c7 25 float error;
MatthewMaat 8:ec3c634390c7 26 float output;
MatthewMaat 8:ec3c634390c7 27 pwm_1.period(1.0/10000); //10kHz
MatthewMaat 7:a32766b96d91 28
MatthewMaat 8:ec3c634390c7 29 while (true) {
MatthewMaat 8:ec3c634390c7 30 degrees_setpoint = setpoint_1 * ain_degrees_range;
MatthewMaat 8:ec3c634390c7 31 error = degrees_setpoint-(motor1.getPosition()*degrees_per_count); //error = setpoint - current
MatthewMaat 8:ec3c634390c7 32 output = error*p_value*kp_1_range;
MatthewMaat 8:ec3c634390c7 33 if(output > 0)
MatthewMaat 8:ec3c634390c7 34 {
MatthewMaat 8:ec3c634390c7 35 direction_1.write(true);
MatthewMaat 8:ec3c634390c7 36 }
MatthewMaat 8:ec3c634390c7 37 else
MatthewMaat 8:ec3c634390c7 38 {
MatthewMaat 8:ec3c634390c7 39 direction_1.write(false);
MatthewMaat 8:ec3c634390c7 40 }
MatthewMaat 8:ec3c634390c7 41 pwm_1.write(fabs(output));
MatthewMaat 8:ec3c634390c7 42 scope.set(0,motor1.getPosition());
MatthewMaat 8:ec3c634390c7 43 scope.set(1,error);
MatthewMaat 8:ec3c634390c7 44 scope.set(2,fabs(output));
MatthewMaat 8:ec3c634390c7 45 led.write(motor1.getPosition()/100.0);
MatthewMaat 8:ec3c634390c7 46 scope.send();
MatthewMaat 8:ec3c634390c7 47 wait(0.01);
MatthewMaat 4:f988679bf9a1 48 }
MatthewMaat 4:f988679bf9a1 49 }