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-26
- Revision:
- 6:1518fccc5616
- Parent:
- 5:0597358d0016
- Child:
- 7:d55569b92f30
File content as of revision 6:1518fccc5616:
//======================================================================= 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.
//*********************************************** FUNCTION DECLARATION *******************************************
//**************************** 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 ***********************************
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.
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.
volatile bool sample_go; // Ticker EMG function.
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 ****************************************
/* Tickers count constantly. The formulas attacht to the ticker count with them. */
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 ==================================================================================================================
//**************************** CONTROL 1-EMG CALIBRATION ***********************************
biquadFilter highpass1(high_a2, high_a3, high_b1, high_b2, high_b3); // Different objects for different inputs, otherwise the v1 and v2 variables get fucked up
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); // filter order is: high-pass --> rectify --> low-pass
y1 = fabs(y1);
y2 = fabs(y2);
y1 = lowpass1.step(y1)*cali_fact1;
y2 = lowpass2.step(y2)*cali_fact2; // roughly normalize to a scale of 0 - 1, where 0 is minimum and 1 is roughly the maximum output.
}
//**************************** TICKER LOOPS ************************************************
void SENSOR_LOOP()
{
Encoderread1 = wheel1.getPulses();
previous_pos_arm = pos_arm;
pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
pos_arm1 = pos_arm;
v_arm = (pos_arm - previous_pos_arm)/dt;
Encoderread2 = wheel2.getPulses();
pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
pos_board1 = pos_board;
flag_s = true;
}
void EMG_LOOP() // ticker function, set flag to true every sample interval
{
if(flag_calibration == false)
{
hoog_laag_filter();
sample_go = 1;
}
}
//================================================================================================== HEAD LOOP ================================================================================================================
int main()
{
pc.baud(115200);
Sensor_control.attach(&SENSOR_LOOP, 0.01); // TICKER FUNCTION
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 is this script 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 ****************************
while(1)
{
if(sample_go)
{
sample_go = 0;
}
if (flag_calibration) // function to calibrate the emg signals from the user. It takes 5 seconds of measurements of maximum output, then takes the max and normalizes to that.
{
calibration_measurements ++;
pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
wait(2);
led_red.write(0);
wait(0.2);
led_red.write(1); //Toggles red calibration LED on
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
// Toggles green sampling LED off
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; // Wachten van fase switch initialiseren
fase_switch_wait = true;
flag_calibration = false;
}
//************************* CONTROL MOTOR ****************************************
if (flag_s)
{
flag_calibration = false;
}
//************************* FASE SWITCH ******************************************
//******************** Fase determination *************
if (fase_switch_wait)
{
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.
pc.printf("waarde j = %i \n",j);
led_red.write(0);
led_green.write(1);
led_yellow.write(0);
}
if( j > N)
{
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)
{
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 = ((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 = ((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.
Encoderread1 = wheel1.getPulses();
pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
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);
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 - (3*calibration_measurements - 3);
pc.printf("Games played total count = %i and loop %i\n",games_played,games_played1);
if(games_played1 == 3)
{
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;
//=================================================== STOP SCRIPT ============================================================
}
}
}
}