Thomas Burgers
/
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
Diff: main.cpp
- Revision:
- 34:c672f5c0763f
- Parent:
- 26:b3693f431d6f
- Child:
- 36:da07b5c2984d
--- a/main.cpp Thu Oct 08 19:17:56 2015 +0000
+++ b/main.cpp Tue Oct 13 21:12:53 2015 +0000
@@ -5,26 +5,59 @@
#include "biquadFilter.h"
#include "encoder.h"
+// ___________________________
+// // \\
+// || [Inputs] ||
+// \\___________________________//
+//
+
+
MODSERIAL pc(USBTX,USBRX);
-DigitalOut debug_led_red(LED_RED); // Debug LED
-DigitalOut debug_led_green(LED_GREEN); // Debug LED
-DigitalOut debug_led_blue(LED_BLUE); // Debug LED
+
+DigitalOut debug_led_red(LED_RED); // Debug LED
+DigitalOut debug_led_green(LED_GREEN); // Debug LED
+DigitalOut debug_led_blue(LED_BLUE); // Debug LED
+
+DigitalIn buttonL1(PTC6); // Button 1 (low on k64f) for testing at the lower board
+DigitalIn buttonL2(PTA4); // Button 2 (low on k64f) for testing at the lower board
+DigitalIn buttonH1(D2); // Button 3 (high on k64f) for testing at the top board
+DigitalIn buttonH2(D6); // Button 4 (high on k64f) for testing at the top board
+
+AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
+
+// ___________________________
+// // \\
+// || [MOTOR TURN] ||
+// \\___________________________//
+//
+
+QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
+PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
+DigitalOut motordirection_turn(D4); // Direction of the motor Turn
+
+// ___________________________
+// // \\
+// || [HIDSCOPE] ||
+// \\___________________________//
+//
HIDScope scope(2); // HIDSCOPE declared
-//Ticker Sample_Ticker; // HIDSCOPE voor main
-//volatile bool sample; // HIDSCOPE voor main
-
-
-// (DEBUGGING AND TESTING BUTTONS) (0 when pressed and 1 when not pressed)
-DigitalIn buttonL1(PTC6); // Button 1 (laag op board) for testing at the lower board
-DigitalIn buttonL2(PTA4); // Button 2 (laag op board) for testing at the lower board
-DigitalIn buttonH1(D2); // Button 3 (hoog op board) for testing at the top board
-DigitalIn buttonH2(D6); // Button 4 (hoog op board) for testing at the top board
+// ___________________________
+// // \\
+// || [CONSTANTS] ||
+// \\___________________________//
+//
volatile bool looptimerflag;
const double cw=0; // zero is clockwise (front view)
const double ccw=1; // one is counterclockwise (front view)
+const int Fs = 512; // sampling frequency (512 Hz)
+const double sample_time = 1/512; // sampling frequency (512 Hz)
+
+// PID motor constants
+double integrate_error_turn=0; // integration error turn motor
+double previous_error_turn=0; // previous error turn motor
const double Gain_P_turn=10; //0.0067;
// stel setpoint tussen (0 en 360) en position tussen (0 en 360)
@@ -32,11 +65,11 @@
// dus 0.1=15*gain gain=0.0067
// Als 3 graden verschil 0.1 dan 0.1/3=gain=0.33
- double Gain_I_turn=0.1; //0.025; //(1/2000) //0.00000134
+double Gain_I_turn=0.1; //0.025; //(1/2000) //0.00000134
// pwm_motor_I=(integrate_error_turn + sample_time*error)*gain; pwm = (4*0.01 + 4)* Gain => 0.1 pwm gewenst (na 1 seconde een verschil van 4 graden)
// 0.1 / (4.01) = Gain = 0.025
- double Gain_D_turn=50; //0.01;
+double Gain_D_turn=50; //0.01;
// error_derivative_turn=(error - previous_error_turn)/sample_time
//
@@ -47,29 +80,28 @@
// degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
-const double sample_time=0.01; // tijd voor een sample (100Hz)
-// PID motor constants
-double integrate_error_turn=0; // integration error turn motor
-double previous_error_turn=0; // previous error turn motor
+// ___________________________
+// // \\
+// || [FUNCTIONS USED] ||
+// \\___________________________//
+//
-
-// Functions used (described after main)
void keep_in_range(double * in, double min, double max);
void setlooptimerflag(void);
double get_reference(double input);
-//void get_sample(void); //HIDSCOPE
+
-// MAIN function
+ ///////////////////////////////
+ // //
+/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
+ // // //
+ /////////////////////////////// //
int main() {
debug_led_red=1;
debug_led_blue=1;
debug_led_green=1;
- AnalogIn potmeter(A0); // Potmeter that can read a reference value (DEBUG TOOL)
- QEI motor_turn(D12,D13,NC,32); // Encoder for motor Turn
- PwmOut pwm_motor_turn(D5); // Pwm for motor Turn
- DigitalOut motordirection_turn(D4); // Direction of the motor Turn
double reference_turn=0; // Set constant to store reference value of the Turn motor
double position_turn; // Set constant to store current position of the Turn motor
double error;
@@ -85,76 +117,84 @@
// Tickers
Ticker looptimer; // Ticker called looptimer to set a looptimerflag
- looptimer.attach(setlooptimerflag,sample_time); // calls the looptimer flag every 0.01s
-
- //Sample_Ticker.attach(&get_sample, sample_time); // HIDSCOPE sample Ticker
+ looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every 0.01s
pc.printf("Start infinite loop \n\r");
wait (3); // Wait before starting system
+
//INFINITE LOOP
while(1)
{ // Start while loop
- // DEBUGGING BUTTON: interrupt button Disbalances the current motor position
- if (buttonL2.read() < 0.5){ //if button pressed
+
+// ___________________________
+// // \\
+// || [DEBUG BUTTONS] ||
+// \\___________________________//
+// interrupt button Disbalances the current motor position
+
+ //if button L2 pressed => disbalance motor \\
+ if (buttonL2.read() < 0.5){
motordirection_turn = cw;
- pwm_motor_turn = 0.5f; // motorspeed
+ pwm_motor_turn = 0.5f;
pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
+
-// // Change Reference button Positive
-// if (buttonH1.read() < 0.5){ //if button pressed
-// pc.printf("Reference after = %d \r\n", reference_turn);
-// reference_turn=reference_turn+45;
-// pc.printf("Reference after = %d \r\n", reference_turn);
-// debug_led = !debug_led; }
-//
-// // Change Reference button Negative
-// if (buttonH2.read() < 0.5){ //if button pressed
-// pc.printf("Reference after = %d \r\n", reference_turn);
-// reference_turn=reference_turn-45;
-// pc.printf("Reference after = %d \r\n", reference_turn);
-// debug_led = !debug_led; }
-
- if (buttonL1.read() < 0.5){ //if button pressed
+ // if button L1 pressed => shut down motor for 1000 seconds \\
+ if (buttonL1.read() < 0.5){
motordirection_turn = cw;
pwm_motor_turn = 0;
wait(1000);
pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
- // Wait until looptimer flag is true then execute PID controller.
- else
+ else
{
+// ___________________________
+// // \\
+// || [Wait for go signal] ||
+// \\___________________________//
+// // Wait until looptimer flag is true then execute PID controller loop. \\
+
while(looptimerflag != true);
looptimerflag = false;
- //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
- //reference_turn = 15; //BOVENAAN IN SCRIPT GEPLAATST
+ //reference = (potmeter.read()-0.5)*2000; // Potmeter bepaald reference (uitgeschakeld)
- // Keep motor position between -4200 and 4200 counts
+// ___________________________
+// // \\
+// || [Calibrate position] ||
+// \\___________________________//
+// // Keep motor position between -4200 and 4200 counts \\
+
if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
{
motor_turn.reset();
pc.printf("RESET \n\r");
}
- // Convert position to degrees
+ // Convert position to degrees \\
+
position_turn = conversion_counts_to_degrees * motor_turn.getPulses();
pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", reference_turn, motor_turn.getPulses(), position_turn);
- // P-CONTROLLER
- // Calculate error then multiply it with the gain, and store in pwm_to_motor
-
- error=(reference_turn - position_turn); // Current error (input controller)
+// ___________________________
+// // \\
+// || [PID CONTROLLER] ||
+// \\___________________________//
+// // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
+
+ error=(reference_turn - position_turn); // Current error (input controller)
- integrate_error_turn=integrate_error_turn + sample_time*error; // integral error output
-// // overwrite previous integrate error by adding the current error multiplied by the sample time.
+ integrate_error_turn=integrate_error_turn + sample_time*error; // integral error output
+// // overwrite previous integrate error by adding the current error
+ // multiplied by the sample time.
//
- double error_derivative_turn=(error - previous_error_turn)/sample_time; // derivative error output
+ double error_derivative_turn=(error - previous_error_turn)/sample_time; // derivative error output
- // FILTER error_derivative_turn (lowpassfilter)
+ // FILTER error_derivative_turn (lowpassfilter)
const double mT_f_a1=-1.965293372622690e+00;
const double mT_f_a2=9.658854605688177e-01;
@@ -177,8 +217,12 @@
pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
-
- // Keep Pwm between -1 and 1
+// ___________________________
+// // \\
+// || [PID error -> pwm motor] ||
+// \\___________________________//
+// // Keep Pwm between -1 and 1 -> and decide the direction of the motor to compensate the error \\
+
keep_in_range(&pwm_to_motor_turn, -1,1); // Pass to motor controller but keep it in range!
pc.printf("pwm %f \n\r", pwm_to_motor_turn);
@@ -194,23 +238,34 @@
pc.printf("else loop pwm < 0 \n\r");
}
- // Put pwm_motor to the motor
+// ___________________________
+// // \\
+// || [pwm -> Plant] ||
+// \\___________________________//
+// // Put pwm to the motor \\
+
pwm_motor_turn=(abs(pwm_to_motor_turn));
-// while(sample != true) // HIDSCOPE input => sample_go nu nog niet nodig opzich // BLINK LEDS TOEVOEGEN
-// {
- //sample_filter; (filter function zie EMG filter working script)
- scope.set(0,reference_turn); // HIDSCOPE channel 0 : Current Reference
- scope.set(0,position_turn); // HIDSCOPE channel 0 : Position_turn
+// ___________________________
+// // \\
+// || [HIDSCOPE] ||
+// \\___________________________//
+// // Check signals inside HIDSCOPE \\
+
+ scope.set(0,reference_turn); // HIDSCOPE channel 0 : Current Reference
+ scope.set(0,position_turn); // HIDSCOPE channel 0 : Position_turn
scope.set(1,pwm_to_motor_turn); // HIDSCOPE channel 1 : Pwm_to_motor_turn
- scope.send(); // Send channel info to HIDSCOPE
-// sample = false;
-// }
- //debug_led = !debug_led; // should flicker with freq 50 Hz
+ scope.send(); // Send channel info to HIDSCOPE
}
}
}
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ // //
+ // [Functions Described] //
+ // //
+ ////////////////////////////////////
+
// Keep in range function
void keep_in_range(double * in, double min, double max)
{
@@ -230,9 +285,3 @@
const float gain = 4.0;
return (input-offset)*gain;
}
-
-//// Get sample
-//void get_sample(void) // HIDSCOPE sample fuction
-//{
-// sample = true;
-//}
