Thomas Burgers / Mbed 2 deprecated ZZ-TheChenneRobot

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:   Encoder HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@3:11a7da46e093, 2015-10-05 (annotated)

Committer:
ThomasBNL
Date:
Mon Oct 05 18:59:14 2015 +0000
Revision:
3:11a7da46e093
Parent:
2:dd46c8f3724a
Child:
4:9fdad59c03d6
waarden van motor 1 lopen tussen de -4200 en 4200;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ThomasBNL 0:40052f5ca77b 1 #include "mbed.h"
ThomasBNL 0:40052f5ca77b 2 //#include "HIDScope.h"
ThomasBNL 0:40052f5ca77b 3 #include "QEI.h"
ThomasBNL 0:40052f5ca77b 4 #include "MODSERIAL.h"
ThomasBNL 0:40052f5ca77b 5 //#include "biquadFilter.h"
ThomasBNL 0:40052f5ca77b 6 #include "encoder.h"
ThomasBNL 0:40052f5ca77b 7
ThomasBNL 3:11a7da46e093 8 /* MODSERIAL to get non-blocking Serial*/
ThomasBNL 3:11a7da46e093 9 MODSERIAL pc(USBTX,USBRX);
ThomasBNL 1:dc683e88b44e 10
ThomasBNL 0:40052f5ca77b 11 void keep_in_range(double * in, double min, double max);
ThomasBNL 0:40052f5ca77b 12
ThomasBNL 0:40052f5ca77b 13 volatile bool looptimerflag;
ThomasBNL 0:40052f5ca77b 14
ThomasBNL 0:40052f5ca77b 15 void setlooptimerflag(void);
ThomasBNL 1:dc683e88b44e 16
ThomasBNL 3:11a7da46e093 17 double get_degrees_from_counts(int counts);
ThomasBNL 1:dc683e88b44e 18
ThomasBNL 1:dc683e88b44e 19 double get_setpoint(double input);
ThomasBNL 0:40052f5ca77b 20
ThomasBNL 0:40052f5ca77b 21 int main() {
ThomasBNL 0:40052f5ca77b 22 //LOCAL VARIABLES
ThomasBNL 0:40052f5ca77b 23 /*Potmeter input*/
ThomasBNL 0:40052f5ca77b 24 AnalogIn potmeter(A0);
ThomasBNL 1:dc683e88b44e 25 QEI motor1(D12,D13,NC,32);
ThomasBNL 0:40052f5ca77b 26 /* PWM control to motor */
ThomasBNL 0:40052f5ca77b 27 PwmOut pwm_motor(D5);
ThomasBNL 0:40052f5ca77b 28 /* Direction pin */
ThomasBNL 0:40052f5ca77b 29 DigitalOut motordir(D4);
ThomasBNL 0:40052f5ca77b 30 /* variable to store setpoint in */
ThomasBNL 0:40052f5ca77b 31 double setpoint;
ThomasBNL 0:40052f5ca77b 32 /* variable to store pwm value in*/
ThomasBNL 0:40052f5ca77b 33 double pwm_to_motor;
ThomasBNL 0:40052f5ca77b 34 /* variable to store position of the motor in */
ThomasBNL 0:40052f5ca77b 35 double position;
ThomasBNL 0:40052f5ca77b 36
ThomasBNL 0:40052f5ca77b 37 //START OF CODE
ThomasBNL 0:40052f5ca77b 38
ThomasBNL 0:40052f5ca77b 39 pc.printf("bla \n\r");
ThomasBNL 0:40052f5ca77b 40
ThomasBNL 0:40052f5ca77b 41 /*Set the baudrate (use this number in RealTerm too! */
ThomasBNL 0:40052f5ca77b 42 pc.baud(9600);
ThomasBNL 0:40052f5ca77b 43
ThomasBNL 0:40052f5ca77b 44 /*Create a ticker, and let it call the */
ThomasBNL 0:40052f5ca77b 45 /*function 'setlooptimerflag' every 0.01s */
ThomasBNL 0:40052f5ca77b 46 Ticker looptimer;
ThomasBNL 0:40052f5ca77b 47
ThomasBNL 0:40052f5ca77b 48 looptimer.attach(setlooptimerflag,0.01); // calls the looptimer flag every 0.01s
ThomasBNL 0:40052f5ca77b 49
ThomasBNL 0:40052f5ca77b 50 pc.printf("bla \n\r");
ThomasBNL 0:40052f5ca77b 51
ThomasBNL 0:40052f5ca77b 52 //INFINITE LOOP
ThomasBNL 0:40052f5ca77b 53
ThomasBNL 0:40052f5ca77b 54 while(1) {
ThomasBNL 0:40052f5ca77b 55 /* Wait until looptimer flag is true. */
ThomasBNL 0:40052f5ca77b 56 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 57
ThomasBNL 0:40052f5ca77b 58 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 59
ThomasBNL 3:11a7da46e093 60 // Setpoint calibration
ThomasBNL 3:11a7da46e093 61 //setpoint = (potmeter.read()-0.5)*2000;
ThomasBNL 3:11a7da46e093 62 setpoint = 15;
ThomasBNL 0:40052f5ca77b 63
ThomasBNL 3:11a7da46e093 64 // Position calibration
ThomasBNL 3:11a7da46e093 65 if ((motor1.getPulses()>4200) || (motor1.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
ThomasBNL 3:11a7da46e093 66 {
ThomasBNL 3:11a7da46e093 67 motor1.reset();
ThomasBNL 3:11a7da46e093 68 pc.printf("RESET \n\r");
ThomasBNL 3:11a7da46e093 69 }
ThomasBNL 0:40052f5ca77b 70
ThomasBNL 3:11a7da46e093 71 position= get_degrees_from_counts(motor1.getPulses());
ThomasBNL 3:11a7da46e093 72
ThomasBNL 0:40052f5ca77b 73
ThomasBNL 3:11a7da46e093 74 pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", setpoint, motor1.getPulses(), position);
ThomasBNL 3:11a7da46e093 75
ThomasBNL 0:40052f5ca77b 76
ThomasBNL 0:40052f5ca77b 77 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
ThomasBNL 3:11a7da46e093 78 // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
ThomasBNL 3:11a7da46e093 79 // max verschil: 360 -> dan pwm_to_motor 1 tot aan een verschil van 15 graden-> bij 15 moet pwm_to_motor ong 0.1 zijn
ThomasBNL 3:11a7da46e093 80 // dus 0.1=15*gain gain=0.0067
ThomasBNL 3:11a7da46e093 81 pwm_to_motor = (setpoint - position)*0.0067;
ThomasBNL 3:11a7da46e093 82
ThomasBNL 0:40052f5ca77b 83
ThomasBNL 2:dd46c8f3724a 84 keep_in_range(&pwm_to_motor, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 3:11a7da46e093 85 pc.printf("pwm %f \n\r", pwm_to_motor);
ThomasBNL 0:40052f5ca77b 86
ThomasBNL 0:40052f5ca77b 87 if(pwm_to_motor > 0)
ThomasBNL 3:11a7da46e093 88 {
ThomasBNL 0:40052f5ca77b 89 motordir=1;
ThomasBNL 3:11a7da46e093 90 pc.printf("if loop pwm_to_motor > 0 \n\r");
ThomasBNL 3:11a7da46e093 91 }
ThomasBNL 0:40052f5ca77b 92 else
ThomasBNL 3:11a7da46e093 93 {
ThomasBNL 3:11a7da46e093 94 motordir=0;
ThomasBNL 3:11a7da46e093 95 pc.printf("else loop pwm_to_motor < 0 \n\r");
ThomasBNL 3:11a7da46e093 96 }
ThomasBNL 0:40052f5ca77b 97 pwm_motor=(abs(pwm_to_motor));
ThomasBNL 0:40052f5ca77b 98 }
ThomasBNL 0:40052f5ca77b 99 }
ThomasBNL 0:40052f5ca77b 100
ThomasBNL 0:40052f5ca77b 101
ThomasBNL 0:40052f5ca77b 102 // Keep in range function
ThomasBNL 0:40052f5ca77b 103 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 104 {
ThomasBNL 0:40052f5ca77b 105 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 106 }
ThomasBNL 0:40052f5ca77b 107
ThomasBNL 0:40052f5ca77b 108 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 109 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 110 {
ThomasBNL 0:40052f5ca77b 111 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 112 }
ThomasBNL 1:dc683e88b44e 113
ThomasBNL 1:dc683e88b44e 114 // Convert Counts -> Rad ===> NOG NIET GEBRUIKT
ThomasBNL 3:11a7da46e093 115 double get_degrees_from_counts(int counts)
ThomasBNL 1:dc683e88b44e 116 {
ThomasBNL 1:dc683e88b44e 117 const int gear_ratio =131;
ThomasBNL 3:11a7da46e093 118 const int ticks_per_magnet_rotation = 32;//X2 Encoder
ThomasBNL 3:11a7da46e093 119 const double degrees_per_encoder_tick =
ThomasBNL 3:11a7da46e093 120 360/(gear_ratio*ticks_per_magnet_rotation);
ThomasBNL 3:11a7da46e093 121 return counts * degrees_per_encoder_tick;
ThomasBNL 1:dc683e88b44e 122 }
ThomasBNL 1:dc683e88b44e 123
ThomasBNL 1:dc683e88b44e 124 // Get setpoint -> potmeter
ThomasBNL 1:dc683e88b44e 125 double get_setpoint(double input)
ThomasBNL 1:dc683e88b44e 126 {
ThomasBNL 1:dc683e88b44e 127 const float offset = 0.5;
ThomasBNL 1:dc683e88b44e 128 const float gain = 4.0;
ThomasBNL 1:dc683e88b44e 129 return (input-offset)*gain;
ThomasBNL 0:40052f5ca77b 130 }