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
main.cpp
- Committer:
- ThomasBNL
- Date:
- 2015-10-14
- Revision:
- 45:359df0594588
- Parent:
- 44:5dd0a3d24662
- Child:
- 46:9279c7a725bf
File content as of revision 45:359df0594588:
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// ___________________________
// // \\
// || [Libraries] ||
// \\___________________________//
//
#include "mbed.h"
#include "HIDScope.h"
#include "QEI.h"
#include "MODSERIAL.h"
#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
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)
Ticker looptimer; // Ticker called looptimer to set a looptimerflag
// ___________________________
// // \\
// || [MOTOR TURN] ||
// \\___________________________//
//
QEI motor_turn(D12,D13,NC,32); // TURN: Encoder for motor
PwmOut pwm_motor_turn(D5); // TURN: Pwm for motor
DigitalOut motordirection_turn(D4); // TURN: Direction of the motor
// PID motor constants
double integrate_error_turn=0; // integration error turn motor
double previous_error_turn=0; // previous error turn motor
// ___________________________
// // \\
// || [MOTOR STRIKE] ||
// \\___________________________//
//QEI motor_strike(XX,XX,NC,32); // STRIKE: Encoder for motor Turn
//PwmOut pwm_motor_strike(XX); // STRIKE: Pwm for motor Turn
//DigitalOut motordirection_strike(XX); // STRIKE: Direction of the motor Turn
// PID motor constants
//double integrate_error_strike=0; // STRIKE: integration error turn motor
//double previous_error_strike=0; // STRIKE: previous error turn motor
// ___________________________
// // \\
// || [HIDSCOPE] ||
// \\___________________________//
//
HIDScope scope(3); // HIDSCOPE declared
// ___________________________
// // \\
// || [System CONSTANTS] ||
// \\___________________________//
//
volatile bool looptimerflag;
const double cw=0; // zero is clockwise (front view)
const double ccw=1; // one is counterclockwise (front view)
const double off=1; // Led off
const double on=0; // Led on
const int Fs = 512; // sampling frequency (512 Hz)
const double sample_time = 0.001953125; // duration of one sample
double conversion_counts_to_degrees=0.085877862594198; // Calculation conversion counts to degrees
// gear ratio motor = 131
// ticks per magnet rotation = 32 (X2 Encoder)
// One revolution = 360 degrees
// degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198
// ___________________________
// // \\
// || [FILTER CONSTANTS] ||
// \\___________________________//
//
const double mT_f_a1=-1.965293372622690e+00; // Motor Turn derivative filter constants
const double mT_f_a2=9.658854605688177e-01;
const double mT_f_b0=1.480219865318266e-04;
const double mT_f_b1=2.960439730636533e-04;
const double mT_f_b2=1.480219865318266e-04;
biquadFilter Lowpassfilter_derivative(mT_f_a1,mT_f_a2,mT_f_b0,mT_f_b1,mT_f_b2); // BiquadFilter used for the filtering of the Derivative action of the PID-action
// ___________________________
// // \\
// || [FUNCTIONS USED] ||
// \\___________________________//
//
void keep_in_range(double * in, double min, double max); // Put in a value and makes sure that the value stays between assigned boundary's
void setlooptimerflag(void); // Sets looptimerflag volatile bool to true
void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain); // STRIKE/TURN: Deactivate PID Controller
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain); // STRIKE: Activate PID Controller
void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain); // TURN: Activate PID Controller
void Change_Turn_Position_Left (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the left
void Change_Turn_Position_Right (double& reference, double change_one_bottle); // TURN: Change Reference position one bottle to the right
///////////////////////////////
// //
/////////////////////////////////////////////////////// [MAIN FUNCTION] ////////////////////////////////////////////////////////////////////////////
// // //
/////////////////////////////// //
int main() {
debug_led_red=off;
debug_led_blue=off;
debug_led_green=off;
const double change_one_bottle=45;
double position_turn; // TURN: Set variable to store current position of the motor
double error_turn; // TURN: Set variable to store the current error of the motor
double pwm_motor_turn_P; // TURN: variable that stores the P action part of the error
double pwm_motor_turn_I; // TURN: variable that stores the I action part of the error
double pwm_motor_turn_D; // TURN: variable that stores the D action part of the error
double pwm_to_motor_turn; // TURN: variable that is constructed by summing the values of the P, I and D action
double P_gain_turn; // TURN: this gain gets multiplied with the error inside the P action of the PID controller
double I_gain_turn; // TURN: this gain gets multiplied with the error inside the I action of the PID controller
double D_gain_turn; // TURN: this gain gets multiplied with the error inside the D action of the PID controller
double reference_turn; // TURN: reference position of the motor (position the motor 'likes' to get to)
//double position_strike; // TURN: Set variable to store current position of the motor
//double error_strike; // TURN: Set variable to store the current error of the motor
//double pwm_motor_strike_P; // STRIKE: variable that stores the P action part of the error
//double pwm_motor_strike_I; // STRIKE: variable that stores the I action part of the error
//double pwm_motor_strike_D; // STRIKE: variable that stores the D action part of the error
//double pwm_to_motor_strike; // STRIKE: variable that is constructed by summing the values of the P, I and D action
//double reference_strike;
//double P_gain_turn; // STRIKE: this gain gets multiplied with the error inside the P action of the PID controller
//double I_gain_turn; // STRIKE: this gain gets multiplied with the error inside the I action of the PID controller
//double D_gain_turn; // STRIKE: this gain gets multiplied with the error inside the D action of the PID controller
//double reference_strike; // STRIKE: reference position of the motor (position the motor 'likes' to get to)
// ___________________________
// // \\
// || [START OF CODE] ||
// \\___________________________//
// START OF CODE
pc.printf("Start of code \n\r");
pc.baud(115200); // Set the baudrate
// ___________________________
// // \\
// || [CALIBRATE] ||
// \\___________________________//
// Calibrate starting postion (RED LED)
pc.printf("Button calibration \n\r");
while(1)
{
debug_led_red=on; // TURN: LOW buttons
if (buttonL2.read() < 0.5){ // TURN: turn the motor clockwise with pwm of 0.2
motordirection_turn = cw;
pwm_motor_turn = 0.2f;}
if (buttonL1.read() < 0.5){ // TURN: turn the motor counterclockwise with pwm of 0.2
motordirection_turn = ccw;
pwm_motor_turn = 0.2f;}
// STRIKE: HIGH buttons
// if (buttonH2.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
// motordirection_strike = cw;
// pwm_motor_turn = 0.2f;}
//
// if (buttonH1.read() < 0.5){ // STRIKE: turn the motor clockwise with pwm of 0.2
// motordirection_strike = ccw;
// pwm_motor_turn = 0.2f;}
//
if ((buttonL2.read() < 0.5) && (buttonL1.read() < 0.5)) // Save current TURN and STRIKE positions as starting position
{
motor_turn.reset(); // TURN: Starting Position
reference_turn=0; // TURN: Set reference position to zero
// motor_strike.reset(); // STRIKE: Starting Position
// double reference_strike=0; // STRIKE: Set reference position to zero
goto calibration_complete; // Calibration complete
}
}
calibration_complete:
debug_led_red=off; // Calibration end => RED LED off
// ___________________________
// // \\
// || [ATTACH TICKER] ||
// \\___________________________//
// Attach Ticker to looptimerflag
looptimer.attach(setlooptimerflag,(float)1/Fs); // calls the looptimer flag every sample
pc.printf("Start infinite loop \n\r");
wait (5); // Wait 5 seconds before starting system
activate_PID_Controller_strike(P_gain_turn, I_gain_turn, D_gain_turn);
// ___________________________
// // \\
// || [START INFINTTE LOOP] ||
// \\___________________________//
//
while(1)
{ // Start while loop
// ___________________________
// // \\
// || [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;
pc.printf("positie = %d \r\n", motor_turn.getPulses()); }
// 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()); }
else
{
// ___________________________
// // \\
// || [Wait for go signal] ||
// \\___________________________//
// // Wait until looptimer flag is true then execute PID controller loop.
while(looptimerflag != true);
looptimerflag = false;
// ___________________________
// // \\
// || [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 \\
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);
// ___________________________
// // \\
// || [PID CONTROLLER] ||
// \\___________________________//
// // Calculate error then multiply it with the (P, I and D) gains, and store in pwm_to_motor \\
error_turn=(reference_turn - position_turn); // TURN: Current error (input controller)
integrate_error_turn=integrate_error_turn + sample_time*error_turn; // TURN: Integral error output
// overwrite previous integrate error by adding the current error
// multiplied by the sample time.
//
double error_derivative_turn=(error_turn - previous_error_turn)/sample_time; // TURN: Derivative error output
error_derivative_turn=Lowpassfilter_derivative.step(error_derivative_turn); // TURN: Filter
previous_error_turn=error_turn; // current error is saved to memory constant to be used in
// the next loop for calculating the derivative error
pwm_motor_turn_P = error_turn*P_gain_turn; // Output P controller to pwm
pwm_motor_turn_I = integrate_error_turn*I_gain_turn; // Output I controller to pwm
pwm_motor_turn_D = error_derivative_turn*D_gain_turn; // Output D controller to pwm
pwm_to_motor_turn = pwm_motor_turn_P + pwm_motor_turn_I + pwm_motor_turn_D;
// ___________________________
// // \\
// || [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);
// Check error and decide direction to turn
if(pwm_to_motor_turn > 0)
{
motordirection_turn=ccw;
pc.printf("if loop pwm > 0 \n\r");
}
else
{
motordirection_turn=cw;
pc.printf("else loop pwm < 0 \n\r");
}
// ___________________________
// // \\
// || [pwm -> Plant] ||
// \\___________________________//
// // Put pwm to the motor \\
pwm_motor_turn=(abs(pwm_to_motor_turn));
// ___________________________
// // \\
// || [HIDSCOPE] ||
// \\___________________________//
// // Check signals inside HIDSCOPE \\
scope.set(0,error_turn); // HIDSCOPE channel 0 : Current Error
scope.set(1,position_turn); // HIDSCOPE channel 1 : Position_turn
scope.set(2,pwm_to_motor_turn); // HIDSCOPE channel 2 : Pwm_to_motor_turn
scope.send(); // Send channel info to HIDSCOPE
// ___________________________
// // \\
// || [Deactivate?] ||
// \\___________________________//
// // Check whether the motor has reached reference position and can be shut down
if(fabs(error_turn)<2) // Shut down if error is smaller than two degrees
{deactivate_PID_Controller(P_gain_turn,I_gain_turn,D_gain_turn);}
else
{activate_PID_Controller_turn(P_gain_turn,I_gain_turn,D_gain_turn);}
}
}
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// //
// [Functions Described] //
// //
////////////////////////////////////
// Keep in range function
void keep_in_range(double * in, double min, double max)
{
*in > min ? *in < max? : *in = max: *in = min;
}
// Looptimerflag function
void setlooptimerflag(void)
{
looptimerflag = true;
}
// Change reference
void Change_Turn_Position_Left (double& reference, double change_one_bottle)
{
if(reference==90)
{
reference=-90;
}
else
{
reference = reference + change_one_bottle;
keep_in_range(&reference, -90, 90); // reference position stays between -90 and 90 degrees (IF bottles at -90, -45, 0, 45, 90 degrees)
}
}
void Change_Turn_Position_Right (double& reference, double change_one_bottle)
{
if(reference==-90)
{
reference=90;
}
else
{
reference = reference - change_one_bottle;
keep_in_range(&reference, -90, 90);
}
}
// Deactivate or Activate PID_Controller
void deactivate_PID_Controller(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0;
I_gain=0;
D_gain=0;
}
void activate_PID_Controller_turn(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0.01; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
I_gain=0.5;
D_gain=5;
}
void activate_PID_Controller_strike(double& P_gain, double& I_gain, double& D_gain)
{
P_gain=0; // hier waardes P,I,D veranderen (waardes bovenaan doen (tijdelijk) niks meer
I_gain=0;
D_gain=0;
}
