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
Diff: main.cpp
- Revision:
- 9:a5c2ddf2cb8e
- Parent:
- 8:151e43b99156
- Child:
- 10:57f090f3ddda
--- a/main.cpp Tue Oct 27 09:57:21 2015 +0000
+++ b/main.cpp Tue Oct 27 15:42:48 2015 +0000
@@ -2,11 +2,10 @@
// 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.
-*/
+ 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.
+//================================================================================ 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.
@@ -15,7 +14,10 @@
#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 *******************************************
+//================================================================================= 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.
@@ -36,7 +38,11 @@
//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 ***********************************
+//========================================================================================= 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.
@@ -68,7 +74,7 @@
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.
@@ -130,13 +136,21 @@
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
+/* 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);
@@ -146,30 +160,33 @@
u1 = EMG1;
u2 = EMG2;
y1 = highpass1.step(u1);
- y2 = highpass2.step(u2); // filter order is: high-pass --> rectify --> low-pass
+ y2 = highpass2.step(u2);
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.
+ y2 = lowpass2.step(y2)*cali_fact2;
}
-//**************************** TICKER LOOPS ************************************************
+//**************************** 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)); // Omrekenen naar graden van 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)); // Omrekenen naar graden van board
+ pos_board = (Encoderread2/((64.0*131.0)/360.0));
pos_board1 = pos_board;
flag_s = true;
}
-void EMG_LOOP() // ticker function, set flag to true every sample interval
+/* This loop determines if the calibration can be done or not. */
+void EMG_LOOP()
{
if(flag_calibration == false)
{
@@ -178,10 +195,15 @@
}
//================================================================================================== 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); // TICKER FUNCTION
+ Sensor_control.attach(&SENSOR_LOOP, 0.01);
EMG_Control.attach(&EMG_LOOP, (float)1/Fs);
led_green.write(0);
@@ -197,23 +219,23 @@
wait(2.5);
pc.printf("===============================================================\n");
- //************************* CONTROL 1-EMG CALIBRATION ****************************
+//**************************** CONTROL 1 - EMG CALIBRATION AND FILTERING ***********************************
while(1)
{
- 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.
+ if (flag_calibration)
{
calibration_measurements ++;
pc.printf(" \n\n EMG Signal starting up Calibration measurement........... \n");
- cali_max1 = 0; // declare max y1
- cali_max2 = 0; //declare max y2
- cali_fact1 = 0.9; // calibration factor to normalize filter output to a scale of 0 - 1
+ 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); //Toggles red calibration LED on
+ led_red.write(1);
wait(0.2);
led_red.write(0);
wait(0.2);
@@ -231,35 +253,34 @@
wait(0.5);
pc.printf("\t......contract muscles..... \n");
- for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++) // Records 5 seconds of y1
+ 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
+ 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
+ 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
+ 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
+ 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);
@@ -277,42 +298,41 @@
y1 = 0;
y2 = 0;
- j = 1; // Wachten van fase switch initialiseren
+ j = 1; // Wait for the fase swith to initiate.
fase_switch_wait = true;
flag_calibration = false;
}
-
- //************************* CONTROL MOTOR ****************************************
- if (flag_s)
+ if (flag_s) // Turns calibration off.
{
flag_calibration = false;
}
- //************************* FASE SWITCH ******************************************
- //******************** Fase determination *************
- if (fase_switch_wait)
+//**************************** 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); // 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.
+ 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)
+ if( j > N) // When the wait is long enough the game starts.
{
fase_switch_wait = false;
switch(fase)
{
- //******************* Fase 1 **************************
+ //************************ Fase 1 ******************************************************************
case(1):
led_red.write(1);
led_green.write(0);
led_yellow.write(0);
rounds = 0;
- if (y1> contract)
- {
+
+ 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;
}
@@ -372,7 +392,8 @@
j = 0;
}
break;
- //******************* Fase 2 **************************
+ //************************ Fase 2 ******************************************************************
+
case(2):
led_red.write(0);
led_green.write(0);
@@ -485,7 +506,7 @@
}
pc.printf("Snelheid arm = \t %f \t EMG1 en EMG2 = %f \t %f \n", speed_control_arm, y1, y2);
break;
- //********************************************* Fase 3 **********************************************
+ //************************ Fase 3 ******************************************************************
case(3):
led_red.write(0);
led_green.write(1);
@@ -586,7 +607,7 @@
break;
}
break;
- //=================================================== STOP SCRIPT ============================================================
+//================================================================================================== END SCRIPT ===============================================================================================================
}
}
}