Biorobotics10
With this script a Ball-E robot can be made and be operative for the use.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Samenvoegen_7_5
by 
Biorobotics10
main.cpp
- Committer:
- EmilyJCZ
- Date:
- 2015-10-27
- Revision:
- 9:a5c2ddf2cb8e
- Parent:
- 8:151e43b99156
- Child:
- 10:57f090f3ddda
File content as of revision 9:a5c2ddf2cb8e:
//======================================================================= Script: Ball-E ==========================================================================
// Authors: Ewoud Velu, Lisa Verhoeven, Robert van der Wal, Thijs van der Wal, Emily Zoetbrood
/* This is the script of a EMG measurment moving robot. The purpose of the robot is to amuse people with the disease of Ducenne.
The robot is designed to throw a ball in to a certain chosen pocket.
In order to achieve this movement we use a ‘arm’ that can turn in the vertical plane and move in the horizontal plane. */
//================================================================================ LIBRARY DECLARATION ==============================================================================================================
/* Libraries are files which contain standard formulas for reading surtain information. Every library contains its own information. */
#include "mbed.h" // Standard library. This includes the reading of AnalogIn, DigitalOut, PwmOut and other standard formules.
#include "QEI.h" // This library includes the reading of of the Encoders from motors.
#include "MODSERIAL.h" // MODSERIAL inherits Serial and adds extensions for buffering.
#include "HIDScope.h" // This sends the measured EMG signal to the HIDScope.
#include "biquadFilter.h" // Because of this library we can make different filters.
#include <cmath> // This library declares a set of functions to compute common mathematical operations and transformations. Only used fabs.
#include <stdio.h> // This library defines three variable types, several macros, and various functions for performing input and output.
//================================================================================= INPUT, OUTPUT AND FUNCTION DECLARATION =============================================================================================
/* Here are different inputs, outputs and functions decleared. There are differnt inputs. For the input and the reading of the encoders.
The outputs are for the different leds, the magnet and the motor. There is also an function decleared for the MODSERIAL and one for the scope.
The one for the scope is in this script turned off because only putty is used but it is possible to turn it on. */
//**************************** INPUTS ******************************************************
AnalogIn EMG1(A0); // Input left biceps EMG.
AnalogIn EMG2(A1); // Input right biceps EMG.
QEI wheel1 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller.
QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller.
//**************************** OUTPUTS *****************************************************
DigitalOut led_red(D1); // Output for red LED decleared.
DigitalOut led_yellow(D3); // Output for yellow LED decleared.
DigitalOut led_green(D9); // Output for green LED delceared.
DigitalOut magnet(D2); // Output for magnet.
DigitalOut motor1direction(D4); // Direction for motor 2 on motorshield. This motor moves the arm in fase 2.
PwmOut motor1speed(D5); // Speed for motor 2 on motorshield. This motor moves the arm in fase 2.
DigitalOut motor2direction(D7); // Direction for motor 1 on motorshield. This motor moves the board in fase 1.
PwmOut motor2speed(D6); // Speed for motor 1 on motorshield. This motor moves the board in fase 1.
//**************************** FUNCTIONS ***************************************************
//HIDScope scope(2); // HIDScope declared with 2 scopes.
MODSERIAL pc(USBTX, USBRX); // Function for Serial communication with the Microcontroller to the pc.
//========================================================================================= GLOBAL VARIABLES DECLARATION =====================================================================================================
/* For every loop there are different variables and constants needed. All these variables and constants are decleared down here. First the constants and variables for the main program.
Second for the control of the motor and than for the different controls and fase switch. At the bottom tickers are defined.
Tickers count constantly. Formulas can be attacht to them so they are contantly calculated. */
const int led_on = 0; // This constant turns the led on.
const int led_off = 1; // This constant turns the led off.
int games_played1 = 0; // Counts number of games played.
int games_played = -1; // Shows real number of games played. There is a -1 because the game is first reset before the first throw.
float dt = 0.01; // Time staps
//**************************** VARIABLES FOR MOTOR CONTROL *********************************
const float cw = 1; // Motor1 moves counter clock wise and Motor2 moves clock wise.
const float ccw = 0; // Motor1 moves clock wise and Motor2 moves counter clock wise.
bool flag_s = false; // Var flag_s sensor ticker
const int relax = 0; // The motor speed is zero.
//**************************** VARIABLES FOR CONTROL 1 *************************************
int Fs = 512; // Sampling frequency.
int calibration_measurements = 0; // Integer counts the number of calibrations done. It starts at 0.
//Filter coefficients. a1 is normalized to 1.
const double low_b1 = /*0.0055427172102806843;*/1.480219865318266e-04;
const double low_b2 = /*0.011085434420561369;*/2.960439730636533e-04;
const double low_b3 = /*0.0055427172102806843; */1.480219865318266e-04;
const double low_a2 = /*-1.778631777824585; */-1.965293372622690e+00;
const double low_a3 = /*0.80080264666570777; */9.658854605688177e-01;
const double high_b1 = /*0.63894552515902237;*/8.047897937631126e-01;
const double high_b2 = /*-1.2778910503180447; */-1.609579587526225e+00;
const double high_b3 = /*0.63894552515902237;*/8.047897937631126e-01;
const double high_a2 = /*-1.1429805025399009;*/-1.571102440190402e+00;
const double high_a3 = /*0.41280159809618855;*/6.480567348620491e-01;
// Declaring the input and output variables.
double u1; // Input from EMG 1. The left biceps.
double y1; // Output from the filters from u1.
double u2; // Input from EMG 2. The right biceps.
double y2; // Output from the filters from u2.
// Declaring variables for calibration
double cali_fact1 = 0.9; // Calibration factor to normalize filter output to a scale of 0 - 1. For signal y1.
double cali_fact2 = 0.9; // Calibration factor to normalize filter output to a scale of 0 - 1. For signal y2.
double cali_max1 = 0; // Declares max y1.
double cali_max2 = 0; // Declares max y2.
double cali_array1[2560] = {}; // Array to store values in for the 5 seconds calibartion for y1.
double cali_array2[2560] = {}; // Array to store values in for the 5 seconds calibartion for y2.
bool flag_calibration = true; // Flag to start the calibration.
//**************************** VARIABLES FOR FASE SWITCH ***********************************
int fase = 3; // The fase is used in a switch to switch between the fases. It starts at the reset fase.
int j = 1; // Wait time. Used in problem1 and fase switch
int N = 200; // Maximum value of j.
bool fase_switch_wait = true; // Timer wait to switch between different fases.
//**************************** VARIABLES FOR CONTROL 2 *************************************
const float contract = 0.5; // The minimum value for y1 and y2 for which the motor moves.
const float fasecontract = 0.7; // The value y1 and y2 must be for the fase change.
const float maxleft = -200; // With this angle the motor should stop moving.
const float maxright = 200; // With this angle the motor should stop moving.
const float speed_plate_1 = 0.1; // The speed of the plate in control 2.
bool flag_left = true; // This flag determines if the signals from the left biceps should be measured. This is signal y1.
bool flag_right = true; // This flag determines if the signals from the right biceps should be measured. This is signal y2.
float pos_board = 0; // The begin position of the board. It begins at zero. Reads Encoder2 in degrees. Formula is applied in SensorTicker.
int pos_board1 = 0; //
float Encoderread2 = 0; // Reads travelled distance from Motor2. Reads pulses.
//**************************** VARIABLES FOR CONTROL 3 *************************************
const float minimum_contract = 0.4; // The minimum value for y2 for whicht the motor moves.
const float medium_contract = 0.5; // Value for different speeds of the motor.
const float maximum_leftbiceps = 0.7; // Value for y1 for which the magnet turns off.
const float on = 1.0; // This value turns the magnet on.
const float off = 0.0; // This value turns the magnet off.
const float minimum_throw_angle = 20; // The minimum angle the arm has to be in to be able to turn the magnet off.
const float maximum_throw_angle = 110; // The maximum angle for the arm to turn the magnet off.
float pos_arm = 0; // The begin position of the arm. It begins at zero. Reads Encoder1 in degrees. Formula is applied in SensorTicker.
int pos_arm1 = 0; // This makes from the positon of the arm degrees within a cirkel as an integer.
float pos_arm2 = 0; // This makes from the positon of the arm degrees within a cirkel as an float.
float previous_pos_arm = 0; // Needed to calculate the v_arm.
float v_arm = 0; // The speed of the arm.
float v_arm_com = 0; // The compensated speed of the arm.
float speed_control_arm = 0.000; //
float Encoderread1 = 0; // Reads travelled distance from Motor1.
int switch_rounds = 2; // Value of a switch to calculate the number of rounds made.
int rounds = 0; // Number of rounds made by the arm.
float pos_armrounds_max = 3; // Max rounds the arm can make.
bool problem1 = false; // Stop for fase 2. It is a wait for problem2 begins.
bool problem2 = false; // The reset of fase 2 when the arm reaches its maximum value.
bool flag_v_arm = false; //
float problem_velocity = 0; //
//**************************** VARIABLES FOR CONTROL 4 *************************************
float reset_position = 0; // The reset position of the arm.
int reset_arm = 0; // The reset position of the arm (?).
int reset_board = 0; // The reset position of the board.
float speedcompensation; // Speed of Motor2 during reset.
float speedcompensation2; // Speed of Motor1 during reset.
int t = 5; // Integer for count down to new game.
int switch_reset = 1; // Value of a switch for the different parts of the reset.
bool board = false; //
// **************************************** Tickers ****************************************
Ticker Sensor_control; // This ticker counts for the position of the motors and the speed of the arm.
Ticker EMG_Control; // This ticker counts for the filtering of the EMG signal
//=============================================================================================== SUB LOOPS ==================================================================================================================
/* Different subloops are used so the head loop doesn't get to full. One loop is used for the filtering and calibration of the EMG signals.
Then there are two loops which are attached to a ticker so they are read through constantly.
One is for the reading of the encoders and one for turning on and off the EMG signal calibration and filtering. */
//**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
/* There are highpass and lowpass filters used. For both the EMG signals is a highpass and a lowpass filter. The function biquadFilter makes these filters.
In the void loop is the filtering done. The u1 and u2 are the reading of the EMG signals. After reading the filters are used. These are the y signals.
The filter order is highpass, recifyer and lowpass. The calibration is done at the same time as the lowpass filter.
The signal is multiplied with the calibarion factor to put the signal between 0 and 1. */
biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3);
biquadFilter highpass2(high_a2, high_a3, high_b1, high_b2, high_b3);
biquadFilter lowpass1(low_a2, low_a3, low_b1, low_b2, low_b3);
biquadFilter lowpass2(low_a2, low_a3, low_b1, low_b2, low_b3);
void hoog_laag_filter()
{
u1 = EMG1;
u2 = EMG2;
y1 = highpass1.step(u1);
y2 = highpass2.step(u2);
y1 = fabs(y1);
y2 = fabs(y2);
y1 = lowpass1.step(y1)*cali_fact1;
y2 = lowpass2.step(y2)*cali_fact2;
}
//**************************** TICKER LOOPS ****************************************************************
/* In the SENSOR_LOOP the signal from the encoders are read. This signal is calculated in degrees. This is the pos_arm and the pos_board.
For the arm there are also spreeds calculated. These are used to give a speed to the motor later on. Also the flag for the calibration is turned off here. */
void SENSOR_LOOP()
{
Encoderread1 = wheel1.getPulses();
previous_pos_arm = pos_arm;
pos_arm = (Encoderread1/((64.0*131.0)/360.0));
pos_arm1 = pos_arm;
v_arm = (pos_arm - previous_pos_arm)/dt;
Encoderread2 = wheel2.getPulses();
pos_board = (Encoderread2/((64.0*131.0)/360.0));
pos_board1 = pos_board;
flag_s = true;
}
/* This loop determines if the calibration can be done or not. */
void EMG_LOOP()
{
if(flag_calibration == false)
{
hoog_laag_filter();
}
}
//================================================================================================== HEAD LOOP ================================================================================================================
/* Here is the main loop defined. First
Then the tickers are attached to the subloops above. Next some lines to turn led off and some lines that are shown in putty.
After this a while loop starts. This loop is always on. It starts with the calibration. Then the board returns to its begin settings.
Then the game can begin in the switch with fase 1, then fase 2 and eventually fase 3. After 5 games the calibration needs to be done again. */
int main()
{
pc.baud(115200);
Sensor_control.attach(&SENSOR_LOOP, 0.01);
EMG_Control.attach(&EMG_LOOP, (float)1/Fs);
led_green.write(0);
led_yellow.write(0);
led_red.write(0);
pc.printf("===============================================================\n");
pc.printf(" \t\t\t Ball-E\n");
pc.printf("In the module Biorobotics on the University of Twente this script is created\n");
pc.printf("Autors:\tE. Velu, L. Verhoeven, R. v/d Wal, T. v/d Wal, E. Zoetbrood\n\n");
wait(3);
pc.printf("The script will begin with a short calibration\n\n");
wait(2.5);
pc.printf("===============================================================\n");
//**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
while(1)
{
if (flag_calibration)
{
calibration_measurements ++;
pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
cali_max1 = 0;
cali_max2 = 0;
cali_fact1 = 0.9;
cali_fact2 = 0.9;
wait(2);
led_red.write(0);
wait(0.2);
led_red.write(1);
wait(0.2);
led_red.write(0);
wait(0.2);
led_red.write(1);
wait(0.2);
led_red.write(0);
wait(0.2);
led_red.write(1);
wait(1);
pc.printf("\t.....Calibrating Signal of EMG 1 and 2 .......\n");
led_red.write(0);
led_yellow.write(1);
wait(0.5);
pc.printf("\t......contract muscles..... \n");
for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) // Records 5 seconds of y1.
{
hoog_laag_filter();
cali_array1[cali_index1] = y1;
wait((float)1/Fs);
}
for(int cali_index2 = 0; cali_index2 < 2560; cali_index2++) // Records 5 seconds of y2.
{
hoog_laag_filter();
cali_array2[cali_index2] = y2;
wait((float)1/Fs);
}
for(int cali_index3 = 0; cali_index3 < 2560; cali_index3++) // Scales y1.
{
if(cali_array1[cali_index3] > cali_max1)
{
cali_max1 = cali_array1[cali_index3];
}
}
for(int cali_index4 = 0; cali_index4 < 2560; cali_index4++) // Scales y2.
{
if(cali_array2[cali_index4] > cali_max2) {
cali_max2 = cali_array2[cali_index4];
}
}
cali_fact1 = (double)1/cali_max1; // Calibration factor for y1.
cali_fact2 = (double)1/cali_max2; // Calibration factor for y2.
led_yellow.write(0);
pc.printf(" \t....... Calibration has been completed!\n");
wait(0.2);
led_green.write(led_off);
wait(0.2);
led_green.write(led_on);
wait(0.2);
led_green.write(led_off);
wait(0.2);
led_green.write(led_on);
wait(4);
pc.printf("Beginning with Ball-E board settings\n");
led_green.write(led_off);
wait(2);
y1 = 0;
y2 = 0;
j = 1; // Wait for the fase swith to initiate.
fase_switch_wait = true;
flag_calibration = false;
}
if (flag_s) // Turns calibration off.
{
flag_calibration = false;
}
//**************************** FASE SWITCH *****************************************************************
if (fase_switch_wait) // There needs to be a wait before the script can start so the sample loop has the time to do the calibration.
{
j++;
wait(0.01);
pc.printf("waarde j = %i \n",j);
led_red.write(0);
led_green.write(1);
led_yellow.write(0);
}
if( j > N) // When the wait is long enough the game starts.
{
fase_switch_wait = false;
switch(fase)
{
//************************ Fase 1 ******************************************************************
case(1):
led_red.write(1);
led_green.write(0);
led_yellow.write(0);
rounds = 0;
if (y1> contract) // If the left arm is above this value flag_left turns true so the motor can run ccw.
{ // The left and right can not be true at the same time so right has to turn off when left is on.
flag_right = false;
flag_left = true;
}
if (y2 > contract)
{
flag_right = true;
flag_left = false;
}
if (pos_board < maxleft)
{
flag_left = false;
motor2speed.write(relax);
}
if (pos_board > maxright)
{
flag_right = false;
motor2speed.write(relax);
}
if (flag_left)
{
if (y1> contract)
{
motor2direction.write(ccw);
motor2speed.write(speed_plate_1);
}
else
{
motor2speed.write(relax);
}
}
if (flag_right)
{
if (y2 > contract)
{
motor2direction.write(cw);
motor2speed.write(speed_plate_1);
}
else
{
motor2speed.write(relax);
}
}
pc.printf("Boardposition \t %f EMG1 en EMG2 signaal = %f \t %f\n", pos_board, y1, y2);
if (y1> fasecontract && y2 > fasecontract)
{
motor2speed.write(relax);
fase = 2;
fase_switch_wait = true;
led_red.write(0);
led_green.write(0);
led_yellow.write(1);
j = 0;
}
break;
//************************ Fase 2 ******************************************************************
case(2):
led_red.write(0);
led_green.write(0);
led_yellow.write(1);
motor1direction.write(cw);
pos_arm1 = (pos_arm - (rounds * 360));
pos_arm2 = pos_arm1;
switch(switch_rounds)
{
case(1):
rounds ++;
switch_rounds = 2;
break;
case(2):
if(pos_arm1>360 & 368<pos_arm1)
{
switch_rounds = 1;
}
break;
}
if (y2 > minimum_contract & y2 < medium_contract)
{
speed_control_arm = 1;//((v_arm/304) + 0.15*(1 -(v_arm/304)));
motor1speed.write(speed_control_arm);
}
if (y2 > medium_contract) // Hoger dan drempelwaarde = Actief
{
speed_control_arm = 1;//((v_arm/304) + 0.15*(1 -(v_arm/304)));
motor1speed.write(speed_control_arm);
}
if (y2 < minimum_contract) // Lager dan drempel = Rust
{
motor1speed.write(relax);
}
if(rounds == pos_armrounds_max) // max aantal draaing gemaakt!!!!!!
{
problem1 = true;
problem2 = true;
motor1speed.write(relax);
while (problem1)
{
j++;
wait(0.01); // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen.
if( j > N)
{
problem1 = false;
}
}
wait(0.1);
led_red.write(0);
wait(0.1);
led_red.write(1);
wait(0.1);
led_red.write(0);
wait(0.1);
led_red.write(1);
wait(0.1);
led_red.write(0);
wait(0.1);
led_red.write(1);
wait(1.5);
while(problem2)
{
motor1direction.write(ccw);
if(pos_arm < 170)
{
if(v_arm > 200)
{
flag_v_arm = true;
}
}
if(flag_v_arm)
{
v_arm_com = v_arm;
}
speed_control_arm = (0.4*((pos_arm2 - reset_arm)/500.0) + (0.05) - (v_arm_com/304)*0.3);
motor1speed.write(speed_control_arm);
if (pos_arm < 10)
{
flag_v_arm = false;
problem2 = false;
motor1speed.write(0);
rounds = 0;
wait(1);
}
}
}
if (pos_arm1 > minimum_throw_angle & pos_arm1 < maximum_throw_angle)
{
if (y1> maximum_leftbiceps)
{
magnet.write(off);
wait(0.1);
motor1speed.write(relax);
fase = 3;
pc.printf("Van fase 2 naar fase 3\n");
wait(2);
j = 0;
fase_switch_wait = true;
}
}
pc.printf("Snelheid arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", speed_control_arm, y1, y2);
break;
//************************ Fase 3 ******************************************************************
case(3):
led_red.write(0);
led_green.write(1);
led_yellow.write(0);
switch(switch_reset)
{
case(1):
if(pos_arm < reset_position) //ene kant op draaien
{
motor1direction.write(cw);
speedcompensation2 = 0.05; //((reset_arm - pos_arm1)/900.0 + (0.02));
motor1speed.write(speedcompensation2);
}
if(pos_arm > reset_position) //andere kant op
{
motor1direction.write(ccw);
speedcompensation2 = 0.05;//((pos_arm1 - reset_arm)/500.0 + (0.02));
motor1speed.write(speedcompensation2);
}
if(pos_arm < reset_position+5 && pos_arm > reset_position-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
{
motor1speed.write(0);
switch_reset = 2;
}
pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
wait(0.0001);
break;
case(2):
pc.printf("\t switch magneet automatisch \n");
wait(0.2);
magnet.write(on);
wait(2);
switch_reset = 3;
break;
case(3):
if(pos_board < reset_board)
{
motor2direction.write(cw);
speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
motor2speed.write(speedcompensation);
}
if(pos_board > reset_board)
{
motor2direction.write(ccw);
speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05));
motor2speed.write(speedcompensation);
}
if(pos_board < reset_board+5 && pos_board > reset_board-5)
{
motor2speed.write(0);
board = true;
}
if(board)
{
pc.printf("fase switch naar 1\n\n");
board = false;
wait(2);
games_played ++;
games_played1 = games_played - (5*calibration_measurements - 5);
pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
if(games_played1 == 5)
{
flag_calibration = true;
while(t)
{
pc.printf("\tCalibratie begint over %i \n",t);
t--;
led_yellow.write(1);
wait(0.5);
led_yellow.write(0);
wait(0.5);
}
}
while(t)
{
pc.printf("\tNieuw spel begint over %i \n",t);
t--;
led_yellow.write(1);
wait(0.5);
led_yellow.write(0);
wait(0.5);
}
fase = 1; // Terug naar fase 1
switch_reset = 1; // De switch op orginele locatie zetten
t = 5;
}
pc.printf("Positie_board = %f \t \t snelheid = %f \n",pos_board, speedcompensation);
wait(0.0001);
break;
}
break;
//================================================================================================== END SCRIPT ===============================================================================================================
}
}
}
}