Ramon Waninge / Mbed 2 deprecated Bioroboticsmerge

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:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Eva Krolis

main.cpp@27:22bfc75f8d1a, 2018-11-01 (annotated)

Committer:
EvaKrolis
Date:
Thu Nov 01 11:17:44 2018 +0000
Revision:
27:22bfc75f8d1a
Parent:
26:efd04dec7710
Child:
28:8bf22ccd11cc
Should work; Not been tested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 15:38258e6b6e91 1 //Libraries
Ramonwaninge 0:779fe292e912 2 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 3 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 4 #include <cmath>
Ramonwaninge 12:8d3bc1fa2321 5 #include "MODSERIAL.h"
Ramonwaninge 13:f77c5f196161 6 #include "BiQuad.h"
Ramonwaninge 13:f77c5f196161 7 #include <algorithm>
Ramonwaninge 25:1123d100d964 8 //#include "FastPWM.h"
Ramonwaninge 0:779fe292e912 9 #define PI 3.14159265
Ramonwaninge 14:e3fe54f0a4b4 10
Ramonwaninge 14:e3fe54f0a4b4 11 //Inputs and outputs
EvaKrolis 20:11fe0aa7f111 12 MODSERIAL pc(USBTX, USBRX); //Connecting to PC
Ramonwaninge 14:e3fe54f0a4b4 13 AnalogIn EMG0In(A0); //EMG input 0
Ramonwaninge 14:e3fe54f0a4b4 14 AnalogIn EMG1In(A1); //EMG input 1
Ramonwaninge 14:e3fe54f0a4b4 15 InterruptIn button(SW3); //Define button
Ramonwaninge 14:e3fe54f0a4b4 16 DigitalOut greenled(LED_GREEN); //Green led
Ramonwaninge 14:e3fe54f0a4b4 17 DigitalOut blueled(LED_BLUE); //Blue led
Ramonwaninge 14:e3fe54f0a4b4 18 DigitalOut redled(LED_RED); //Red led
Ramonwaninge 13:f77c5f196161 19
Ramonwaninge 15:38258e6b6e91 20 //Constants
Ramonwaninge 25:1123d100d964 21 const float ll = 230; //Length of the lower arm
Ramonwaninge 25:1123d100d964 22 const float lu = 198; //Length of the upper arm
Ramonwaninge 25:1123d100d964 23 const float lb = 50; //Length of the part between the upper arms
Ramonwaninge 25:1123d100d964 24 const float le = 79; //Length of the end-effector beam
Ramonwaninge 25:1123d100d964 25 const float xbase = 450-lb; //Length between the motors
Ramonwaninge 15:38258e6b6e91 26 const int Length = 10000; //Length of the array for the calibration
Ramonwaninge 15:38258e6b6e91 27 const int Parts = 50; //Mean average filter over 50 values
Ramonwaninge 15:38258e6b6e91 28
Ramonwaninge 14:e3fe54f0a4b4 29 //parameters for kinematics
Ramonwaninge 25:1123d100d964 30 float theta1 = PI*0.49; //Angle of the left motor
Ramonwaninge 25:1123d100d964 31 float theta4 = PI*0.49; //Angle of the right motor
Ramonwaninge 25:1123d100d964 32 float thetaflip = 0; //Angle of the flipping motor
Ramonwaninge 25:1123d100d964 33 float prefx; //Desired x velocity
Ramonwaninge 25:1123d100d964 34 float prefy; //Desired y velocity "
Ramonwaninge 26:efd04dec7710 35 float dt = 0.002; //Time step of the system
Ramonwaninge 26:efd04dec7710 36 float iJ[2][2]; //inverse Jacobian matrix
Ramonwaninge 25:1123d100d964 37 //Time step (dependent on sample frequency)
EvaKrolis 20:11fe0aa7f111 38
EvaKrolis 20:11fe0aa7f111 39 //Kinematics parameters for x
Ramonwaninge 25:1123d100d964 40 float xendsum;
Ramonwaninge 25:1123d100d964 41 float xendsqrt1;
Ramonwaninge 25:1123d100d964 42 float xendsqrt2;
Ramonwaninge 25:1123d100d964 43 float xend;
Ramonwaninge 25:1123d100d964 44 float jacobiana;
Ramonwaninge 25:1123d100d964 45 float jacobianc;
EvaKrolis 20:11fe0aa7f111 46
EvaKrolis 20:11fe0aa7f111 47 //Kinematics parameters for y
Ramonwaninge 25:1123d100d964 48 float yendsum;
Ramonwaninge 25:1123d100d964 49 float yendsqrt1;
Ramonwaninge 25:1123d100d964 50 float yendsqrt2;
Ramonwaninge 25:1123d100d964 51 float yend;
Ramonwaninge 25:1123d100d964 52 float jacobianb;
Ramonwaninge 25:1123d100d964 53 float jacobiand;
Ramonwaninge 14:e3fe54f0a4b4 54
Ramonwaninge 14:e3fe54f0a4b4 55 //Parameters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 56 float EMG0; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 57 float EMG0filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 58 float EMG0filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 59 float EMG0Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 60 float Sum0 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 61 float EMG0Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 62 int ReadCal0 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 63 float MaxValue0 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 64 float Threshold0 = 0; //Threshold for the first EMG signal
Ramonwaninge 13:f77c5f196161 65
Ramonwaninge 14:e3fe54f0a4b4 66 //Parameters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 67 float EMG1; //float for EMG input
Ramonwaninge 14:e3fe54f0a4b4 68 float EMG1filt; //float for filtered EMG
Ramonwaninge 14:e3fe54f0a4b4 69 float EMG1filtArray[Parts]; //Array for the filtered array
Ramonwaninge 14:e3fe54f0a4b4 70 float EMG1Average; //float for the value after Moving Average Filter
Ramonwaninge 14:e3fe54f0a4b4 71 float Sum1 = 0; //Sum0 for the moving average filter
Ramonwaninge 15:38258e6b6e91 72 float EMG1Calibrate[Length]; //Array for the calibration
Ramonwaninge 14:e3fe54f0a4b4 73 int ReadCal1 = 0; //Integer to read over the calibration array
Ramonwaninge 14:e3fe54f0a4b4 74 float MaxValue1 = 0; //float to save the max muscle
Ramonwaninge 14:e3fe54f0a4b4 75 float Threshold1 = 0; //Threshold for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 76
Ramonwaninge 14:e3fe54f0a4b4 77 //Filter variables
EvaKrolis 27:22bfc75f8d1a 78 BiQuad Notch50_0(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
EvaKrolis 27:22bfc75f8d1a 79 BiQuad Notch50_1(0.9049,-1.4641,0.9049,-1.4641,0.8098); //Make Notch filter around 50 Hz
EvaKrolis 27:22bfc75f8d1a 80 BiQuad Notch100_0(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
EvaKrolis 27:22bfc75f8d1a 81 BiQuad Notch100_1(0.8427,-0.5097,0.8247,-0.5097,0.6494); //Make Notch filter around 100 Hz
EvaKrolis 27:22bfc75f8d1a 82 BiQuad Notch150_0(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
EvaKrolis 27:22bfc75f8d1a 83 BiQuad Notch150_1(0.7548,0.4665,0.7544,0.4665,0.5095); //Make Notch filter around 150 Hz
EvaKrolis 27:22bfc75f8d1a 84 BiQuad Notch200_0(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
EvaKrolis 27:22bfc75f8d1a 85 BiQuad Notch200_1(0.6919,1.1196,0.6919,1.1196,0.3839); //Make Notch filter around 200 Hz
EvaKrolis 27:22bfc75f8d1a 86 BiQuad High_0(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
EvaKrolis 27:22bfc75f8d1a 87 BiQuad High_1(0.9150,-1.8299,0.9150,-1.8227,0.8372); //Make high-pass filter
EvaKrolis 27:22bfc75f8d1a 88 BiQuad Low_0(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
EvaKrolis 27:22bfc75f8d1a 89 BiQuad Low_1(0.6389,1.2779,0.6389,1.143,0.4128); //Make low-pass filter
Ramonwaninge 14:e3fe54f0a4b4 90 BiQuadChain filter0; //Make chain of filters for the first EMG signal
Ramonwaninge 14:e3fe54f0a4b4 91 BiQuadChain filter1; //Make chain of filters for the second EMG signal
Ramonwaninge 14:e3fe54f0a4b4 92
Ramonwaninge 13:f77c5f196161 93 //Timers and Tickers
Ramonwaninge 13:f77c5f196161 94 Ticker FindMax0_timer; //Timer for finding the max muscle
Ramonwaninge 13:f77c5f196161 95 Ticker FindMax1_timer; //Timer for finding the max muscle
Ramonwaninge 25:1123d100d964 96 Timer local_timer;
EvaKrolis 27:22bfc75f8d1a 97 Ticker hoofdticker;
Ramonwaninge 13:f77c5f196161 98
Ramonwaninge 14:e3fe54f0a4b4 99 //Bool for movement
Ramonwaninge 14:e3fe54f0a4b4 100 bool xMove = false; //Bool for the x-movement
Ramonwaninge 14:e3fe54f0a4b4 101 bool yMove = false; //Bool for the y-movement
Ramonwaninge 14:e3fe54f0a4b4 102
Ramonwaninge 14:e3fe54f0a4b4 103 //Parameters for the state machine
Ramonwaninge 14:e3fe54f0a4b4 104 enum States {Calibration, WorkingMode}; //Initialize state machine
EvaKrolis 20:11fe0aa7f111 105 States CurrentState = Calibration; //Start in the calibration mode
Ramonwaninge 14:e3fe54f0a4b4 106 bool StateBool = true; //Bool to first go in a state
Ramonwaninge 14:e3fe54f0a4b4 107 bool SwitchStateBool = false; //Bool to switch from calibration to working mode
Ramonwaninge 14:e3fe54f0a4b4 108
Ramonwaninge 14:e3fe54f0a4b4 109 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 110 void ReadUseEMG0()
Ramonwaninge 15:38258e6b6e91 111 {
Ramonwaninge 15:38258e6b6e91 112 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 14:e3fe54f0a4b4 113 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 14:e3fe54f0a4b4 114 }
Ramonwaninge 15:38258e6b6e91 115
Ramonwaninge 15:38258e6b6e91 116 Sum0 = 0;
Ramonwaninge 15:38258e6b6e91 117 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 118 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 15:38258e6b6e91 119 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 120 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 121
Ramonwaninge 15:38258e6b6e91 122 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 123 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 124 }
Ramonwaninge 15:38258e6b6e91 125 EMG0Average = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 126
Ramonwaninge 15:38258e6b6e91 127 if (EMG0Average > Threshold0) { //If the value is higher than the threshold value
EvaKrolis 27:22bfc75f8d1a 128 redled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 129 xMove = true; //Set movement to true
Ramonwaninge 15:38258e6b6e91 130 } else {
EvaKrolis 27:22bfc75f8d1a 131 redled = 1; //Otherwise turn the LED off
Ramonwaninge 15:38258e6b6e91 132 xMove = false; //Otherwise set movement to false
Ramonwaninge 15:38258e6b6e91 133 }
Ramonwaninge 15:38258e6b6e91 134 }
Ramonwaninge 15:38258e6b6e91 135 //Function to read and filter the EMG
Ramonwaninge 15:38258e6b6e91 136 void ReadUseEMG1()
Ramonwaninge 15:38258e6b6e91 137 {
Ramonwaninge 15:38258e6b6e91 138 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 139 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 140 }
Ramonwaninge 15:38258e6b6e91 141
Ramonwaninge 15:38258e6b6e91 142 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 143 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 144 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 145 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 146 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 147
Ramonwaninge 15:38258e6b6e91 148 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 149 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 15:38258e6b6e91 150 }
Ramonwaninge 15:38258e6b6e91 151 EMG1Average = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 152
Ramonwaninge 15:38258e6b6e91 153 if (EMG1Average > Threshold1) { //If the value is higher than the threshold value
Ramonwaninge 15:38258e6b6e91 154 greenled = 0; //Turn the LED on
Ramonwaninge 15:38258e6b6e91 155 yMove = true; //Set y movement to true
Ramonwaninge 15:38258e6b6e91 156 } else {
Ramonwaninge 15:38258e6b6e91 157 greenled = 1; //Otherwise turn the LED off
EvaKrolis 16:deb42ce3c3a1 158 yMove = false; //Otherwise set y movement to false
Ramonwaninge 15:38258e6b6e91 159 }
Ramonwaninge 15:38258e6b6e91 160 }
Ramonwaninge 15:38258e6b6e91 161
Ramonwaninge 15:38258e6b6e91 162
Ramonwaninge 15:38258e6b6e91 163 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 164 void CalibrateEMG0()
Ramonwaninge 15:38258e6b6e91 165 {
Ramonwaninge 15:38258e6b6e91 166 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 167 EMG0filtArray[i] = EMG0filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 168 }
Ramonwaninge 15:38258e6b6e91 169
Ramonwaninge 14:e3fe54f0a4b4 170 Sum0 = 0;
Ramonwaninge 14:e3fe54f0a4b4 171 EMG0 = EMG0In; //Save EMG input in float
Ramonwaninge 14:e3fe54f0a4b4 172 EMG0filt = filter0.step(EMG0); //Filter the signal
Ramonwaninge 14:e3fe54f0a4b4 173 EMG0filt = abs(EMG0filt); //Take the absolute value
Ramonwaninge 14:e3fe54f0a4b4 174 EMG0filtArray[0] = EMG0filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 175
Ramonwaninge 15:38258e6b6e91 176 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 14:e3fe54f0a4b4 177 Sum0 += EMG0filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 178 }
Ramonwaninge 15:38258e6b6e91 179 EMG0Calibrate[ReadCal0] = (float)Sum0/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 180
Ramonwaninge 15:38258e6b6e91 181 ReadCal0++;
Ramonwaninge 15:38258e6b6e91 182 }
Ramonwaninge 15:38258e6b6e91 183
Ramonwaninge 15:38258e6b6e91 184 //Function to make an array during the calibration
Ramonwaninge 15:38258e6b6e91 185 void CalibrateEMG1()
Ramonwaninge 15:38258e6b6e91 186 {
Ramonwaninge 15:38258e6b6e91 187 for(int i = Parts ; i > 0 ; i--) { //Make a first in, first out array
Ramonwaninge 15:38258e6b6e91 188 EMG1filtArray[i] = EMG1filtArray[i-1]; //Every value moves one up
Ramonwaninge 15:38258e6b6e91 189 }
Ramonwaninge 15:38258e6b6e91 190
Ramonwaninge 15:38258e6b6e91 191 Sum1 = 0;
Ramonwaninge 15:38258e6b6e91 192 EMG1 = EMG1In; //Save EMG input in float
Ramonwaninge 15:38258e6b6e91 193 EMG1filt = filter1.step(EMG1); //Filter the signal
Ramonwaninge 15:38258e6b6e91 194 EMG1filt = abs(EMG1filt); //Take the absolute value
Ramonwaninge 15:38258e6b6e91 195 EMG1filtArray[0] = EMG1filt; //Save the filtered signal on the first place in the array
Ramonwaninge 15:38258e6b6e91 196
Ramonwaninge 15:38258e6b6e91 197 for(int i = 0 ; i < Parts ; i++) { //Moving Average filter
Ramonwaninge 15:38258e6b6e91 198 Sum1 += EMG1filtArray[i]; //Sum the new value and the previous 49
Ramonwaninge 14:e3fe54f0a4b4 199 }
Ramonwaninge 15:38258e6b6e91 200 EMG1Calibrate[ReadCal1] = (float)Sum1/Parts; //Divide the sum by 50
Ramonwaninge 15:38258e6b6e91 201
Ramonwaninge 15:38258e6b6e91 202 ReadCal1++;
Ramonwaninge 15:38258e6b6e91 203 }
Ramonwaninge 15:38258e6b6e91 204
Ramonwaninge 15:38258e6b6e91 205 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 206 void FindMax0()
Ramonwaninge 15:38258e6b6e91 207 {
Ramonwaninge 15:38258e6b6e91 208 MaxValue0 = *max_element(EMG0Calibrate+500,EMG0Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 209 Threshold0 = 0.30f*MaxValue0; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 210 pc.printf("The calibration value of the first EMG is %f.\n\r The threshold is %f. \n\r",MaxValue0,Threshold0); //Print the max value and the threshold
Ramonwaninge 15:38258e6b6e91 211 FindMax0_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 212 }
Ramonwaninge 15:38258e6b6e91 213
Ramonwaninge 15:38258e6b6e91 214 //Function to find the max value during the calibration
Ramonwaninge 15:38258e6b6e91 215 void FindMax1()
Ramonwaninge 15:38258e6b6e91 216 {
Ramonwaninge 15:38258e6b6e91 217 MaxValue1 = *max_element(EMG1Calibrate+500,EMG1Calibrate+Length); //Find max value, but discard the first 100 values
Ramonwaninge 15:38258e6b6e91 218 Threshold1 = 0.30f*MaxValue1; //The threshold is a percentage of the max value
Ramonwaninge 15:38258e6b6e91 219 pc.printf("The calibration value of the second EMG is %f.\n\r The threshold is %f. \n\r",MaxValue1,Threshold1); //Print the Max value and the threshold
Ramonwaninge 15:38258e6b6e91 220 FindMax1_timer.detach(); //Detach the timer, so you only use this once
Ramonwaninge 15:38258e6b6e91 221 }
Ramonwaninge 15:38258e6b6e91 222
Ramonwaninge 15:38258e6b6e91 223 //Function to stop the reading of the EMG
Ramonwaninge 15:38258e6b6e91 224 void StopProgram()
Ramonwaninge 15:38258e6b6e91 225 {
Ramonwaninge 15:38258e6b6e91 226 greenled = 1; //Turn the LEDs off
Ramonwaninge 15:38258e6b6e91 227 blueled = 1;
Ramonwaninge 15:38258e6b6e91 228 redled = 1;
Ramonwaninge 15:38258e6b6e91 229 exit (0); //Abort mission!!
Ramonwaninge 15:38258e6b6e91 230 }
Ramonwaninge 15:38258e6b6e91 231
Ramonwaninge 15:38258e6b6e91 232 //Function to switch a state
Ramonwaninge 25:1123d100d964 233 /*void SwitchState()
Ramonwaninge 15:38258e6b6e91 234 {
Ramonwaninge 15:38258e6b6e91 235 SwitchStateBool = true; //Set the bool for the start of a state to true
Ramonwaninge 15:38258e6b6e91 236 SwitchState_timer.detach(); //Use this function once
Ramonwaninge 25:1123d100d964 237 }*/
Ramonwaninge 14:e3fe54f0a4b4 238
Ramonwaninge 26:efd04dec7710 239 //forward kinematics function , &xend and&yend are output.
Ramonwaninge 25:1123d100d964 240 void FK(float &xend_, float &yend_, float theta1_, float theta4_)
Ramonwaninge 25:1123d100d964 241 {
Ramonwaninge 26:efd04dec7710 242
Ramonwaninge 26:efd04dec7710 243 //Below we have the forward kinematics formula. Input should be the measured angles theta1 &theta4. Output
Ramonwaninge 25:1123d100d964 244 float xendsum_ = lb + xbase +ll*(cos(theta1_) - cos(theta4_));
Ramonwaninge 25:1123d100d964 245 float xendsqrt1_ = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1_)+cos(theta4_))/2) -ll*(1+ cos(theta1_+theta4_)))*(-sin(theta1_)+sin(theta4_));
Ramonwaninge 25:1123d100d964 246 float xendsqrt2_ = sqrt(pow((-xbase/ll+cos(theta1_)+cos(theta4_)),2)+ pow(sin(theta1_) - sin(theta4_),2));
Ramonwaninge 25:1123d100d964 247 xend_ = (xendsum_ + xendsqrt1_/xendsqrt2_)/2;
Ramonwaninge 26:efd04dec7710 248
Ramonwaninge 25:1123d100d964 249 float yendsum_ = -le + ll/2*(sin(theta1_)+sin(theta4_));
Ramonwaninge 25:1123d100d964 250 float yendsqrt1_ = (-xbase/ll + cos(theta1_)+cos(theta4_))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1_)+cos(theta4_))- ll*(1+cos(theta1_+theta4_))));
Ramonwaninge 25:1123d100d964 251 float yendsqrt2_ = sqrt(pow((-xbase/ll + cos(theta1_)+ cos(theta4_)),2)+ pow((sin(theta1_)-sin(theta4_)),2));
Ramonwaninge 25:1123d100d964 252 yend_ = (yendsum_ + yendsqrt1_/yendsqrt2_);
Ramonwaninge 25:1123d100d964 253 }
Ramonwaninge 14:e3fe54f0a4b4 254
Ramonwaninge 26:efd04dec7710 255 //Below we have the inverse kinematics function.
Ramonwaninge 25:1123d100d964 256 void inverse(float prex, float prey)
Ramonwaninge 15:38258e6b6e91 257 {
Ramonwaninge 26:efd04dec7710 258 theta1 += (prefx*(iJ[0][0])+iJ[0][1]*prey)*dt; //theta 1 is itself + the desired speeds in x and y direction, both
Ramonwaninge 26:efd04dec7710 259 theta4 += (prefx*iJ[1][0]+iJ[1][1]*prey)*dt; //multiplied with a prefactor which comes out of the motor
Ramonwaninge 26:efd04dec7710 260 //the iJ values are defined in the "kinematics" function
Ramonwaninge 26:efd04dec7710 261
Ramonwaninge 26:efd04dec7710 262 //Calling the forward kinematics, to calculate xend and yend
Ramonwaninge 25:1123d100d964 263 FK(xend,yend,theta1,theta4);
Ramonwaninge 12:8d3bc1fa2321 264
Ramonwaninge 3:de8d3ca44a3e 265 }
Ramonwaninge 25:1123d100d964 266
Ramonwaninge 12:8d3bc1fa2321 267 void kinematics()
Ramonwaninge 12:8d3bc1fa2321 268 {
Ramonwaninge 12:8d3bc1fa2321 269
Ramonwaninge 25:1123d100d964 270 float xend1,xend2,xend3,yend1,yend2,yend3;
Ramonwaninge 25:1123d100d964 271 const float dq = 0.0001;
Ramonwaninge 25:1123d100d964 272 FK(xend1,yend1,theta1,theta4);
Ramonwaninge 25:1123d100d964 273 FK(xend2,yend2,theta1+dq,theta4);
Ramonwaninge 25:1123d100d964 274 FK(xend3,yend3,theta1,theta4+dq);
Ramonwaninge 25:1123d100d964 275
Ramonwaninge 25:1123d100d964 276 float a,b,c,d;
Ramonwaninge 25:1123d100d964 277 a = xend2-xend1;
Ramonwaninge 25:1123d100d964 278 b = xend3-xend1;
Ramonwaninge 25:1123d100d964 279 c = yend2-yend1;
Ramonwaninge 25:1123d100d964 280 d = yend3-yend1;
Ramonwaninge 25:1123d100d964 281
Ramonwaninge 25:1123d100d964 282 float Q = 1/(a*d-b*c)/dq;
Ramonwaninge 25:1123d100d964 283
Ramonwaninge 25:1123d100d964 284
Ramonwaninge 25:1123d100d964 285 iJ[0][0] = d*Q;
Ramonwaninge 25:1123d100d964 286 iJ[0][1]= -c*Q;
Ramonwaninge 25:1123d100d964 287 iJ[1][0] = -b*Q;
Ramonwaninge 25:1123d100d964 288 iJ[1][1] = a*Q;
Ramonwaninge 26:efd04dec7710 289
Ramonwaninge 26:efd04dec7710 290
Ramonwaninge 26:efd04dec7710 291 prefx = 1*xMove; //sw3, Prefx has multiplier one, but that has to become a value dependant on the motor
Ramonwaninge 26:efd04dec7710 292 prefy = 1*yMove; //sw2,
Ramonwaninge 12:8d3bc1fa2321 293 inverse(prefx, prefy);
Ramonwaninge 12:8d3bc1fa2321 294 }
Ramonwaninge 12:8d3bc1fa2321 295
Ramonwaninge 26:efd04dec7710 296 // these values are printed for controlling purposes (can later be removed)
EvaKrolis 16:deb42ce3c3a1 297 void printvalue()
EvaKrolis 16:deb42ce3c3a1 298 {
Ramonwaninge 26:efd04dec7710 299 pc.printf("X-value: %f \t Y-value: %f \n\r \t theta 1 = %f \t theta4 = %f",xend, yend,theta1,theta4);
Ramonwaninge 26:efd04dec7710 300 }
Ramonwaninge 12:8d3bc1fa2321 301
Ramonwaninge 15:38258e6b6e91 302 //State Machine
Ramonwaninge 15:38258e6b6e91 303 void StateMachine()
Ramonwaninge 15:38258e6b6e91 304 {
Ramonwaninge 15:38258e6b6e91 305 switch(CurrentState) { //Determine in which state you are
Ramonwaninge 15:38258e6b6e91 306
Ramonwaninge 25:1123d100d964 307 case Calibration:
Ramonwaninge 25:1123d100d964 308 //Calibration mode
Ramonwaninge 15:38258e6b6e91 309 if(StateBool) { //If you go into this state
Ramonwaninge 15:38258e6b6e91 310 pc.printf("You can start calibrating. \n\r"); //Print that you are in this state
Ramonwaninge 15:38258e6b6e91 311 StateBool = false; //Set the bool for the start of a state to false
Ramonwaninge 15:38258e6b6e91 312 FindMax0_timer.attach(&FindMax0,15); //Find the maximum value after 15 seconds
Ramonwaninge 15:38258e6b6e91 313 FindMax1_timer.attach(&FindMax1,15); //Find the maximum value after 15 seconds
EvaKrolis 27:22bfc75f8d1a 314 //local_timer.reset();
Ramonwaninge 25:1123d100d964 315 local_timer.start();
Ramonwaninge 15:38258e6b6e91 316 blueled = 0;
Ramonwaninge 15:38258e6b6e91 317 }
Ramonwaninge 15:38258e6b6e91 318
Ramonwaninge 26:efd04dec7710 319 CalibrateEMG0(); //start emg calibration every 0.005 seconds
Ramonwaninge 25:1123d100d964 320 CalibrateEMG1(); //Start EMG calibration every 0.005 seconds
Ramonwaninge 25:1123d100d964 321
Ramonwaninge 25:1123d100d964 322 if (local_timer.read() > 20) { //If the bool is changed
Ramonwaninge 15:38258e6b6e91 323 CurrentState = WorkingMode; //Change the state to the working mode
Ramonwaninge 15:38258e6b6e91 324 StateBool = true; //Set the start of a state bool to true
Ramonwaninge 15:38258e6b6e91 325 }
Ramonwaninge 15:38258e6b6e91 326 break;
Ramonwaninge 15:38258e6b6e91 327
Ramonwaninge 15:38258e6b6e91 328 case WorkingMode: //Mode to get the robot working
Ramonwaninge 15:38258e6b6e91 329 if (StateBool) { //If you start to go in this state
Ramonwaninge 15:38258e6b6e91 330 pc.printf("You are know in the working mode. \r\n"); //Print in which mode you are
Ramonwaninge 15:38258e6b6e91 331 StateBool = false; //Set the start of state bool to true
Ramonwaninge 15:38258e6b6e91 332 }
Ramonwaninge 25:1123d100d964 333 blueled = 1;
Ramonwaninge 25:1123d100d964 334
Ramonwaninge 26:efd04dec7710 335 ReadUseEMG0();//Start the use of EMG
Ramonwaninge 26:efd04dec7710 336 ReadUseEMG1();//Start the use of EMG
Ramonwaninge 26:efd04dec7710 337 kinematics(); //Starts calculating the x and y value of the endeffector, as well as the desired values and their BIJBEHORENDE angles
Ramonwaninge 25:1123d100d964 338
Ramonwaninge 25:1123d100d964 339 //motorcontroller
Ramonwaninge 25:1123d100d964 340 //Set the blue led off
EvaKrolis 16:deb42ce3c3a1 341 //pc.printf("First EMG: %f, Second EMG: %f \n\r",EMG0Average,EMG1Average);
Ramonwaninge 15:38258e6b6e91 342 break;
Ramonwaninge 15:38258e6b6e91 343 }
Ramonwaninge 15:38258e6b6e91 344 }
Ramonwaninge 15:38258e6b6e91 345
Ramonwaninge 0:779fe292e912 346 int main()
Ramonwaninge 0:779fe292e912 347 {
Ramonwaninge 26:efd04dec7710 348 FK(xend,yend,theta1,theta4);
Ramonwaninge 2:0a7a3c0c08d3 349 pc.baud(115200);
Ramonwaninge 13:f77c5f196161 350 greenled = 1; //First turn the LEDs off
Ramonwaninge 13:f77c5f196161 351 blueled = 1;
Ramonwaninge 13:f77c5f196161 352 redled = 1;
EvaKrolis 27:22bfc75f8d1a 353 filter0.add(&Notch50_0).add(&Notch100_0).add(&Notch150_0).add(&Notch200_0).add(&Low_0).add(&High_0); //Make filter chain for the first EMG
EvaKrolis 27:22bfc75f8d1a 354 filter1.add(&Notch50_1).add(&Notch100_1).add(&Notch150_1).add(&Notch200_1).add(&Low_1).add(&High_1); //Make filter chain for the second EMG
Ramonwaninge 13:f77c5f196161 355 button.rise(StopProgram); //If the button is pressed, stop program
Ramonwaninge 25:1123d100d964 356 hoofdticker.attach(&StateMachine,0.002);
Ramonwaninge 12:8d3bc1fa2321 357 while(true) {
Ramonwaninge 25:1123d100d964 358 printvalue();
Ramonwaninge 25:1123d100d964 359 wait(0.75);
Ramonwaninge 8:697aa3c94209 360 }
Ramonwaninge 0:779fe292e912 361 }