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@2:dd46c8f3724a, 2015-10-05 (annotated)

Committer:
ThomasBNL
Date:
Mon Oct 05 16:33:15 2015 +0000
Revision:
2:dd46c8f3724a
Parent:
1:dc683e88b44e
Child:
3:11a7da46e093
Potmeter werkend om setpoint in te dienen

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 1:dc683e88b44e 8 const double Pi=3.14;
ThomasBNL 1:dc683e88b44e 9
ThomasBNL 0:40052f5ca77b 10 void keep_in_range(double * in, double min, double max);
ThomasBNL 0:40052f5ca77b 11
ThomasBNL 0:40052f5ca77b 12 volatile bool looptimerflag;
ThomasBNL 0:40052f5ca77b 13
ThomasBNL 0:40052f5ca77b 14 void setlooptimerflag(void);
ThomasBNL 1:dc683e88b44e 15
ThomasBNL 1:dc683e88b44e 16 double get_radians_from_counts(int counts);
ThomasBNL 1:dc683e88b44e 17
ThomasBNL 1:dc683e88b44e 18 double get_setpoint(double input);
ThomasBNL 0:40052f5ca77b 19
ThomasBNL 0:40052f5ca77b 20 int main() {
ThomasBNL 0:40052f5ca77b 21 //LOCAL VARIABLES
ThomasBNL 0:40052f5ca77b 22 /*Potmeter input*/
ThomasBNL 0:40052f5ca77b 23 AnalogIn potmeter(A0);
ThomasBNL 1:dc683e88b44e 24 QEI motor1(D12,D13,NC,32);
ThomasBNL 0:40052f5ca77b 25 /* MODSERIAL to get non-blocking Serial*/
ThomasBNL 0:40052f5ca77b 26 MODSERIAL pc(USBTX,USBRX);
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 0:40052f5ca77b 55 while(1) {
ThomasBNL 0:40052f5ca77b 56 /* Wait until looptimer flag is true. */
ThomasBNL 0:40052f5ca77b 57 while(looptimerflag != true);
ThomasBNL 0:40052f5ca77b 58
ThomasBNL 0:40052f5ca77b 59 looptimerflag = false;
ThomasBNL 0:40052f5ca77b 60
ThomasBNL 1:dc683e88b44e 61 setpoint = get_setpoint(potmeter.read());
ThomasBNL 0:40052f5ca77b 62
ThomasBNL 1:dc683e88b44e 63 pc.printf("setpoint: %d, position motor %d \n\r", setpoint, motor1.getPulses());
ThomasBNL 0:40052f5ca77b 64
ThomasBNL 1:dc683e88b44e 65 position=motor1.getPulses();
ThomasBNL 0:40052f5ca77b 66
ThomasBNL 0:40052f5ca77b 67 keep_in_range(&position,-4200,4200);
ThomasBNL 0:40052f5ca77b 68
ThomasBNL 0:40052f5ca77b 69 pc.printf("pwm before keep in range: %d \n\r", position);
ThomasBNL 0:40052f5ca77b 70
ThomasBNL 0:40052f5ca77b 71 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
ThomasBNL 2:dd46c8f3724a 72 pwm_to_motor = ((setpoint*1000) - position)*.001;
ThomasBNL 0:40052f5ca77b 73
ThomasBNL 0:40052f5ca77b 74 pc.printf("pwm before keep in range: %d \n\r", pwm_to_motor);
ThomasBNL 0:40052f5ca77b 75
ThomasBNL 2:dd46c8f3724a 76 keep_in_range(&pwm_to_motor, -1,1); // Pass to motor controller but keep it in range!
ThomasBNL 0:40052f5ca77b 77
ThomasBNL 0:40052f5ca77b 78 pc.printf("pwm after keep in range: %d \n\r", pwm_to_motor);
ThomasBNL 0:40052f5ca77b 79
ThomasBNL 0:40052f5ca77b 80 if(pwm_to_motor > 0)
ThomasBNL 0:40052f5ca77b 81 motordir=1;
ThomasBNL 0:40052f5ca77b 82 else
ThomasBNL 0:40052f5ca77b 83 motordir=0;
ThomasBNL 0:40052f5ca77b 84 pwm_motor=(abs(pwm_to_motor));
ThomasBNL 0:40052f5ca77b 85 }
ThomasBNL 0:40052f5ca77b 86 }
ThomasBNL 0:40052f5ca77b 87
ThomasBNL 0:40052f5ca77b 88
ThomasBNL 0:40052f5ca77b 89 // Keep in range function
ThomasBNL 0:40052f5ca77b 90 void keep_in_range(double * in, double min, double max)
ThomasBNL 0:40052f5ca77b 91 {
ThomasBNL 0:40052f5ca77b 92 *in > min ? *in < max? : *in = max: *in = min;
ThomasBNL 0:40052f5ca77b 93 }
ThomasBNL 0:40052f5ca77b 94
ThomasBNL 0:40052f5ca77b 95 // Looptimerflag function
ThomasBNL 0:40052f5ca77b 96 void setlooptimerflag(void)
ThomasBNL 0:40052f5ca77b 97 {
ThomasBNL 0:40052f5ca77b 98 looptimerflag = true;
ThomasBNL 1:dc683e88b44e 99 }
ThomasBNL 1:dc683e88b44e 100
ThomasBNL 1:dc683e88b44e 101 // Convert Counts -> Rad ===> NOG NIET GEBRUIKT
ThomasBNL 1:dc683e88b44e 102 double get_radians_from_counts(int counts)
ThomasBNL 1:dc683e88b44e 103 {
ThomasBNL 1:dc683e88b44e 104 const int gear_ratio =131;
ThomasBNL 1:dc683e88b44e 105 const int ticks_per_magnet_rotation = 32;//X2
ThomasBNL 1:dc683e88b44e 106 const double radian_per_encoder_tick =
ThomasBNL 1:dc683e88b44e 107 2*Pi/(gear_ratio*ticks_per_magnet_rotation);
ThomasBNL 1:dc683e88b44e 108 return counts * radian_per_encoder_tick;
ThomasBNL 1:dc683e88b44e 109 }
ThomasBNL 1:dc683e88b44e 110
ThomasBNL 1:dc683e88b44e 111 // Get setpoint -> potmeter
ThomasBNL 1:dc683e88b44e 112 double get_setpoint(double input)
ThomasBNL 1:dc683e88b44e 113 {
ThomasBNL 1:dc683e88b44e 114 const float offset = 0.5;
ThomasBNL 1:dc683e88b44e 115 const float gain = 4.0;
ThomasBNL 1:dc683e88b44e 116 return (input-offset)*gain;
ThomasBNL 0:40052f5ca77b 117 }