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@7:ddd7fb357786, 2015-10-06 (annotated)

Committer:
ThomasBNL
Date:
Tue Oct 06 11:59:45 2015 +0000
Revision:
7:ddd7fb357786
Parent:
6:8a62f76a1f68
Child:
8:50d6e2323d3b
Set CW en CCW en added some comments

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