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@6:8a62f76a1f68, 2015-10-05 (annotated)

Committer:
ThomasBNL
Date:
Mon Oct 05 19:14:19 2015 +0000
Revision:
6:8a62f76a1f68
Parent:
5:8fb74a22fe3c
Child:
7:ddd7fb357786
WERKEND P-controller met verschil in positie in graden;

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