Lars van der Hoeven / Mbed 2 deprecated final_build

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 QEI biquadFilter mbed

main.cpp@0:66d33cc8318f, 2018-11-06 (annotated)

Committer:
lars123
Date:
Tue Nov 06 16:23:09 2018 +0000
Revision:
0:66d33cc8318f
lol klaar; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lars123 0:66d33cc8318f 1 #include "mbed.h"
lars123 0:66d33cc8318f 2 #include "MODSERIAL.h"
lars123 0:66d33cc8318f 3 #include "math.h"
lars123 0:66d33cc8318f 4 #include "BiQuad.h"
lars123 0:66d33cc8318f 5 #include "QEI.h"
lars123 0:66d33cc8318f 6
lars123 0:66d33cc8318f 7 //Inputs outputs e.t.c.
lars123 0:66d33cc8318f 8
lars123 0:66d33cc8318f 9 //Serial
lars123 0:66d33cc8318f 10 MODSERIAL pc(USBTX, USBRX);
lars123 0:66d33cc8318f 11
lars123 0:66d33cc8318f 12 //Leds
lars123 0:66d33cc8318f 13 DigitalOut led1(LED_GREEN);
lars123 0:66d33cc8318f 14 DigitalOut led2(LED_BLUE);
lars123 0:66d33cc8318f 15 DigitalOut led3(LED_RED);
lars123 0:66d33cc8318f 16
lars123 0:66d33cc8318f 17 //Servo's
lars123 0:66d33cc8318f 18 DigitalOut StirUp(D2);
lars123 0:66d33cc8318f 19 DigitalOut StirDown(D3);
lars123 0:66d33cc8318f 20
lars123 0:66d33cc8318f 21
lars123 0:66d33cc8318f 22 //Timing
lars123 0:66d33cc8318f 23 Ticker LedTimer;
lars123 0:66d33cc8318f 24 Timeout LedTimeOut;
lars123 0:66d33cc8318f 25
lars123 0:66d33cc8318f 26 Ticker ScopeTimer;
lars123 0:66d33cc8318f 27 Timeout SignalTimeout;
lars123 0:66d33cc8318f 28
lars123 0:66d33cc8318f 29 Ticker ControlTicker;
lars123 0:66d33cc8318f 30 Ticker ControlTicker1;
lars123 0:66d33cc8318f 31
lars123 0:66d33cc8318f 32 //Buttons / Pots
lars123 0:66d33cc8318f 33 DigitalIn button1(D0);
lars123 0:66d33cc8318f 34 DigitalIn button2(D1);
lars123 0:66d33cc8318f 35 AnalogIn Pot1(A5);
lars123 0:66d33cc8318f 36 AnalogIn Pot2(A4);
lars123 0:66d33cc8318f 37
lars123 0:66d33cc8318f 38 //Emg
lars123 0:66d33cc8318f 39 AnalogIn emg1(A0);
lars123 0:66d33cc8318f 40 AnalogIn emg2(A1);
lars123 0:66d33cc8318f 41 AnalogIn emg3(A2);
lars123 0:66d33cc8318f 42
lars123 0:66d33cc8318f 43
lars123 0:66d33cc8318f 44 //Control
lars123 0:66d33cc8318f 45 PwmOut Motor2PWR(D5);
lars123 0:66d33cc8318f 46 DigitalOut Motor2DIR(D4);
lars123 0:66d33cc8318f 47 PwmOut Motor1PWR(D6);
lars123 0:66d33cc8318f 48 DigitalOut Motor1DIR(D7);
lars123 0:66d33cc8318f 49 QEI Encoder1(D12,D13,NC,4200);
lars123 0:66d33cc8318f 50 QEI Encoder2(D10,D11,NC,4200);
lars123 0:66d33cc8318f 51
lars123 0:66d33cc8318f 52 //Filter
lars123 0:66d33cc8318f 53 BiQuadChain bqc;
lars123 0:66d33cc8318f 54
lars123 0:66d33cc8318f 55
lars123 0:66d33cc8318f 56
lars123 0:66d33cc8318f 57 //Declare variables
lars123 0:66d33cc8318f 58 double fsample = 500; //EMG sample frequency Hz
lars123 0:66d33cc8318f 59 double fcontrol = 1000; //Control frequency Hz
lars123 0:66d33cc8318f 60 int max_n; // length of measured arrays
lars123 0:66d33cc8318f 61 double pi = 3.14159265; // define PI
lars123 0:66d33cc8318f 62 int Cteller = 0; //some int variables that I use to count in some functions. Important for if statements.
lars123 0:66d33cc8318f 63 int Cteller1 = 0;
lars123 0:66d33cc8318f 64 int Cnum = 0;
lars123 0:66d33cc8318f 65 int signalsC = 0;
lars123 0:66d33cc8318f 66
lars123 0:66d33cc8318f 67 // variables that depend on fsample
lars123 0:66d33cc8318f 68 double Ts = (double)1/(double)1000; // Sample time in seconds of control loop
lars123 0:66d33cc8318f 69 double emg1_log[500*6]; // allocating space for the EMG arrays
lars123 0:66d33cc8318f 70 double emg2_log[500*6];
lars123 0:66d33cc8318f 71 double emg3_log[500*6];
lars123 0:66d33cc8318f 72
lars123 0:66d33cc8318f 73 double RadPerCount = 2*pi/4200; // turning counts into rads using this value
lars123 0:66d33cc8318f 74
lars123 0:66d33cc8318f 75 //Encoders
lars123 0:66d33cc8318f 76 int counts1;
lars123 0:66d33cc8318f 77 int counts2;
lars123 0:66d33cc8318f 78
lars123 0:66d33cc8318f 79 //PID
lars123 0:66d33cc8318f 80 double refpos1 = 1.48; //angles for reference positions
lars123 0:66d33cc8318f 81 double refpos2 = 1.05;
lars123 0:66d33cc8318f 82 double Kp1 = 25; //values for first PID controller
lars123 0:66d33cc8318f 83 double Ki1 = 13;
lars123 0:66d33cc8318f 84 double Kd1 = 25;
lars123 0:66d33cc8318f 85
lars123 0:66d33cc8318f 86 double Kp2 = 25; //values for second controller
lars123 0:66d33cc8318f 87 double Ki2 = 13;
lars123 0:66d33cc8318f 88 double Kd2 = 25;
lars123 0:66d33cc8318f 89
lars123 0:66d33cc8318f 90 //Movavg //variables needed for the moving average filter
lars123 0:66d33cc8318f 91 int avgn;
lars123 0:66d33cc8318f 92 double avgh1;
lars123 0:66d33cc8318f 93 double avgh2;
lars123 0:66d33cc8318f 94 double avgh3;
lars123 0:66d33cc8318f 95 double ms = 100; // number of datapoints used in moving average filter
lars123 0:66d33cc8318f 96 double highnum1;
lars123 0:66d33cc8318f 97 double highnum2;
lars123 0:66d33cc8318f 98 double highnum3;
lars123 0:66d33cc8318f 99
lars123 0:66d33cc8318f 100 //Bools //all kinds of bools used in if statements They specify if some actions are allowed or not
lars123 0:66d33cc8318f 101 bool stirred = false;
lars123 0:66d33cc8318f 102 bool MeasureBool = true;
lars123 0:66d33cc8318f 103 bool MoveBool = true;
lars123 0:66d33cc8318f 104 bool CalibratedE = false;
lars123 0:66d33cc8318f 105 volatile bool CalibratedM = false;
lars123 0:66d33cc8318f 106 volatile bool Homing;
lars123 0:66d33cc8318f 107 bool BlinkBool;
lars123 0:66d33cc8318f 108 volatile bool pause_loop = true;
lars123 0:66d33cc8318f 109 bool start = true;
lars123 0:66d33cc8318f 110
lars123 0:66d33cc8318f 111 //States //there are 3 statemachines. 1. LED colors 2.states 3.directions
lars123 0:66d33cc8318f 112 bool statechanged = false;
lars123 0:66d33cc8318f 113 bool logcq;
lars123 0:66d33cc8318f 114 enum Colors {Green, Blue, Red, Yellow, Purple, Off};
lars123 0:66d33cc8318f 115 enum states {CalibrateM, TestStand, Coffee, CalibrateE, MandM, Done};
lars123 0:66d33cc8318f 116 enum Directions {Up, Down, Left, Right};
lars123 0:66d33cc8318f 117 Directions CurrentDir = Down;
lars123 0:66d33cc8318f 118 Colors LedState = Off;
lars123 0:66d33cc8318f 119 states ProgramState = CalibrateM;
lars123 0:66d33cc8318f 120
lars123 0:66d33cc8318f 121 //Systeem eigenschappen /arm lengths in meters
lars123 0:66d33cc8318f 122 double L0 = 0.39; //cm
lars123 0:66d33cc8318f 123 double L1 = 0.295; //dm
lars123 0:66d33cc8318f 124
lars123 0:66d33cc8318f 125 //Speeds and Positions //joint positions and speeds also desired velocities
lars123 0:66d33cc8318f 126 double setpoint = 0;
lars123 0:66d33cc8318f 127 double MaxV = 1;//6.3; //rad/s
lars123 0:66d33cc8318f 128 double qv[2]; //setpoint velocity
lars123 0:66d33cc8318f 129 double qr[2]; //setpoint pos
lars123 0:66d33cc8318f 130 double q1;
lars123 0:66d33cc8318f 131 double q2;
lars123 0:66d33cc8318f 132 int dir_i; //int used during the changing of direction
lars123 0:66d33cc8318f 133 double dir_c;
lars123 0:66d33cc8318f 134 double cqlog[2500]; //can be used to log measured joint positions
lars123 0:66d33cc8318f 135
lars123 0:66d33cc8318f 136 //Maak filters
lars123 0:66d33cc8318f 137 BiQuad bq1(0.9475454809720195,-1.895090961944039,0.9475454809720195,-1.8932193507849706,0.8969625731031077); //highpass 5Hz
lars123 0:66d33cc8318f 138 BiQuad bq2(0.843582000346111,-1.3649443488576334,0.843582000346111,-1.3649443488576334,0.6871640006922219); //notch 50Hz
lars123 0:66d33cc8318f 139 BiQuad bq3(0.9824,-0.6071,0.9824,-0.6071,0.9647); //notch 100Hz
lars123 0:66d33cc8318f 140 BiQuad bq4(0.9738 , 0.6018 , 0.9738,0.6018 , 0.9475); //notch 150Hz
lars123 0:66d33cc8318f 141 BiQuad bq5(0.9653 , 1.5619 , 0.9653,1.5619 , 0.9307); //notch 200Hz
lars123 0:66d33cc8318f 142
lars123 0:66d33cc8318f 143 //Programma
lars123 0:66d33cc8318f 144
lars123 0:66d33cc8318f 145 void CancelPause(){ // used to cancel a pause loop
lars123 0:66d33cc8318f 146 pause_loop = false;
lars123 0:66d33cc8318f 147 }
lars123 0:66d33cc8318f 148
lars123 0:66d33cc8318f 149 void ClearEmg(){ // Clears all EMG arrays and variables ascociated with them (moving avarage for example)
lars123 0:66d33cc8318f 150 int n;
lars123 0:66d33cc8318f 151 for(n = 0; n <= max_n; n++){
lars123 0:66d33cc8318f 152 emg1_log[n] = 0;
lars123 0:66d33cc8318f 153 }
lars123 0:66d33cc8318f 154 for(n = 0; n <= max_n; n++){
lars123 0:66d33cc8318f 155 emg2_log[n] = 0;
lars123 0:66d33cc8318f 156 }
lars123 0:66d33cc8318f 157 for(n = 0; n <= max_n; n++){
lars123 0:66d33cc8318f 158 emg3_log[n] = 0;
lars123 0:66d33cc8318f 159 }
lars123 0:66d33cc8318f 160 avgh1 = 0;
lars123 0:66d33cc8318f 161 avgh2 = 0;
lars123 0:66d33cc8318f 162 avgh3 = 0;
lars123 0:66d33cc8318f 163 avgn = 0;
lars123 0:66d33cc8318f 164 max_n = 0;
lars123 0:66d33cc8318f 165 }
lars123 0:66d33cc8318f 166
lars123 0:66d33cc8318f 167 void MeasurePos(){ //function to measure the current joint angles
lars123 0:66d33cc8318f 168 if(CalibratedM){
lars123 0:66d33cc8318f 169 counts1 = Encoder1.getPulses();
lars123 0:66d33cc8318f 170 counts2 = Encoder2.getPulses();
lars123 0:66d33cc8318f 171 q1 = (pi/(double)4)+RadPerCount*counts1;
lars123 0:66d33cc8318f 172 q2 = (pi/(double)4)-RadPerCount*counts2;
lars123 0:66d33cc8318f 173 }
lars123 0:66d33cc8318f 174 else{
lars123 0:66d33cc8318f 175 counts1 = Encoder1.getPulses();
lars123 0:66d33cc8318f 176 counts2 = Encoder2.getPulses();
lars123 0:66d33cc8318f 177 q1 = RadPerCount*counts1;
lars123 0:66d33cc8318f 178 q2 = -RadPerCount*counts2;
lars123 0:66d33cc8318f 179 }
lars123 0:66d33cc8318f 180 }
lars123 0:66d33cc8318f 181
lars123 0:66d33cc8318f 182 void ReferencePos(){ //function to turn reference velocities into reference positions
lars123 0:66d33cc8318f 183 if(CalibratedM){
lars123 0:66d33cc8318f 184 qr[0] += qv[0]*1/fcontrol;
lars123 0:66d33cc8318f 185 qr[1] += qv[1]*1/fcontrol;
lars123 0:66d33cc8318f 186 }
lars123 0:66d33cc8318f 187 }
lars123 0:66d33cc8318f 188
lars123 0:66d33cc8318f 189 void fmovavg(){ //moving average filter
lars123 0:66d33cc8318f 190 double avgm1;
lars123 0:66d33cc8318f 191 double avgm2;
lars123 0:66d33cc8318f 192 double avgm3;
lars123 0:66d33cc8318f 193 avgh1 += emg1_log[max_n];
lars123 0:66d33cc8318f 194 avgh2 += emg2_log[max_n];
lars123 0:66d33cc8318f 195 avgh3 += emg3_log[max_n];
lars123 0:66d33cc8318f 196 if(max_n >= ms){
lars123 0:66d33cc8318f 197 avgm1 = emg1_log[avgn];
lars123 0:66d33cc8318f 198 avgm2 = emg2_log[avgn];
lars123 0:66d33cc8318f 199 avgm3 = emg3_log[avgn];
lars123 0:66d33cc8318f 200 emg1_log[avgn] = avgh1/ms;
lars123 0:66d33cc8318f 201 emg2_log[avgn] = avgh2/ms;
lars123 0:66d33cc8318f 202 emg3_log[avgn] = avgh3/ms;
lars123 0:66d33cc8318f 203 avgh1 -= avgm1;
lars123 0:66d33cc8318f 204 avgh2 -= avgm2;
lars123 0:66d33cc8318f 205 avgh3 -= avgm3;
lars123 0:66d33cc8318f 206 avgn++;
lars123 0:66d33cc8318f 207 }
lars123 0:66d33cc8318f 208 }
lars123 0:66d33cc8318f 209
lars123 0:66d33cc8318f 210 void EmgMeasure(){ //meet de EMG waarden en stop ze in een array
lars123 0:66d33cc8318f 211 if(MeasureBool && CalibratedE){
lars123 0:66d33cc8318f 212 emg1_log[max_n] = fabs(bqc.step(emg1.read()))/highnum1;
lars123 0:66d33cc8318f 213 emg2_log[max_n] = fabs(bqc.step(emg2.read()))/highnum2;
lars123 0:66d33cc8318f 214 emg3_log[max_n] = fabs(bqc.step(emg2.read()))/highnum3;
lars123 0:66d33cc8318f 215 fmovavg();
lars123 0:66d33cc8318f 216 max_n ++;
lars123 0:66d33cc8318f 217 }
lars123 0:66d33cc8318f 218 else if(MeasureBool){
lars123 0:66d33cc8318f 219 emg1_log[max_n] = fabs(bqc.step(emg1.read()));
lars123 0:66d33cc8318f 220 emg2_log[max_n] = fabs(bqc.step(emg2.read()));
lars123 0:66d33cc8318f 221 emg3_log[max_n] = fabs(bqc.step(emg3.read()));
lars123 0:66d33cc8318f 222 fmovavg();
lars123 0:66d33cc8318f 223 max_n++;
lars123 0:66d33cc8318f 224 }
lars123 0:66d33cc8318f 225 }
lars123 0:66d33cc8318f 226
lars123 0:66d33cc8318f 227 void MakeEmg(){ //was used during testing. the function creates a predetermined array that can be analised
lars123 0:66d33cc8318f 228 int n;
lars123 0:66d33cc8318f 229 for(n =0 ;n<=max_n;n++){
lars123 0:66d33cc8318f 230 if((n >= 500) && (n <= 600)){
lars123 0:66d33cc8318f 231 emg1_log[n] = 0.3;
lars123 0:66d33cc8318f 232 }
lars123 0:66d33cc8318f 233 else{
lars123 0:66d33cc8318f 234 emg1_log[n] = 0;
lars123 0:66d33cc8318f 235 }
lars123 0:66d33cc8318f 236 }
lars123 0:66d33cc8318f 237 }
lars123 0:66d33cc8318f 238
lars123 0:66d33cc8318f 239 void LedBlink(){ //function that regulates the LED colors
lars123 0:66d33cc8318f 240 switch(LedState){
lars123 0:66d33cc8318f 241 case Green:
lars123 0:66d33cc8318f 242 if(BlinkBool){
lars123 0:66d33cc8318f 243 led1.write(!led1.read());
lars123 0:66d33cc8318f 244 }
lars123 0:66d33cc8318f 245 else{
lars123 0:66d33cc8318f 246 led1.write(0);
lars123 0:66d33cc8318f 247 }
lars123 0:66d33cc8318f 248 led2.write(1);
lars123 0:66d33cc8318f 249 led3.write(1);
lars123 0:66d33cc8318f 250 break;
lars123 0:66d33cc8318f 251 case Blue:
lars123 0:66d33cc8318f 252 if(BlinkBool){
lars123 0:66d33cc8318f 253 led2.write(!led2.read());
lars123 0:66d33cc8318f 254 }
lars123 0:66d33cc8318f 255 else{
lars123 0:66d33cc8318f 256 led2.write(0);
lars123 0:66d33cc8318f 257 }
lars123 0:66d33cc8318f 258 led1.write(1);
lars123 0:66d33cc8318f 259 led3.write(1);
lars123 0:66d33cc8318f 260 break;
lars123 0:66d33cc8318f 261 case Red:
lars123 0:66d33cc8318f 262 if(BlinkBool){
lars123 0:66d33cc8318f 263 led3.write(!led3.read());
lars123 0:66d33cc8318f 264 }
lars123 0:66d33cc8318f 265 else{
lars123 0:66d33cc8318f 266 led3.write(0);
lars123 0:66d33cc8318f 267 }
lars123 0:66d33cc8318f 268 led1.write(1);
lars123 0:66d33cc8318f 269 led2.write(1);
lars123 0:66d33cc8318f 270 break;
lars123 0:66d33cc8318f 271 case Yellow:
lars123 0:66d33cc8318f 272 if(BlinkBool){
lars123 0:66d33cc8318f 273 led3.write(!led3.read());
lars123 0:66d33cc8318f 274 led1.write(!led1.read());
lars123 0:66d33cc8318f 275 }
lars123 0:66d33cc8318f 276 else{
lars123 0:66d33cc8318f 277 led3.write(0);
lars123 0:66d33cc8318f 278 led1.write(0);
lars123 0:66d33cc8318f 279 }
lars123 0:66d33cc8318f 280 led2.write(1);
lars123 0:66d33cc8318f 281 break;
lars123 0:66d33cc8318f 282 case Purple:
lars123 0:66d33cc8318f 283 if(BlinkBool){
lars123 0:66d33cc8318f 284 led3.write(!led3.read());
lars123 0:66d33cc8318f 285 led2.write(!led1.read());
lars123 0:66d33cc8318f 286 }
lars123 0:66d33cc8318f 287 else{
lars123 0:66d33cc8318f 288 led3.write(0);
lars123 0:66d33cc8318f 289 led2.write(0);
lars123 0:66d33cc8318f 290 }
lars123 0:66d33cc8318f 291 led1.write(1);
lars123 0:66d33cc8318f 292 break;
lars123 0:66d33cc8318f 293 default:
lars123 0:66d33cc8318f 294 led1.write(1);
lars123 0:66d33cc8318f 295 led2.write(1);
lars123 0:66d33cc8318f 296 led3.write(1);
lars123 0:66d33cc8318f 297 break;
lars123 0:66d33cc8318f 298 }
lars123 0:66d33cc8318f 299 }
lars123 0:66d33cc8318f 300
lars123 0:66d33cc8318f 301 void LedSet(){ //function that changes LED colors based on direction of movement
lars123 0:66d33cc8318f 302 if(CurrentDir == Up){
lars123 0:66d33cc8318f 303 LedState = Red;
lars123 0:66d33cc8318f 304 }
lars123 0:66d33cc8318f 305 else if(CurrentDir == Down){
lars123 0:66d33cc8318f 306 LedState = Blue;
lars123 0:66d33cc8318f 307 }
lars123 0:66d33cc8318f 308 else if(CurrentDir == Left){
lars123 0:66d33cc8318f 309 LedState = Green;
lars123 0:66d33cc8318f 310 }
lars123 0:66d33cc8318f 311 else if(CurrentDir == Right){
lars123 0:66d33cc8318f 312 LedState = Yellow;
lars123 0:66d33cc8318f 313 }
lars123 0:66d33cc8318f 314 //LedState = Off;
lars123 0:66d33cc8318f 315 }
lars123 0:66d33cc8318f 316
lars123 0:66d33cc8318f 317 void CheckDIR(){ //old function that was designed to change direction. IS NOT USED ANYMORE
lars123 0:66d33cc8318f 318 if(dir_c >= 0.8 && dir_i<=10){
lars123 0:66d33cc8318f 319 if(dir_i >= 1){
lars123 0:66d33cc8318f 320 //LedState = Blue;
lars123 0:66d33cc8318f 321 LedBlink();
lars123 0:66d33cc8318f 322 }
lars123 0:66d33cc8318f 323 else if(dir_i >= 5){
lars123 0:66d33cc8318f 324 //LedState = Red;
lars123 0:66d33cc8318f 325 LedBlink();
lars123 0:66d33cc8318f 326 }
lars123 0:66d33cc8318f 327 dir_i++;
lars123 0:66d33cc8318f 328 }
lars123 0:66d33cc8318f 329 else{
lars123 0:66d33cc8318f 330 if(dir_i >= 10 && CurrentDir == (Up || Down)){
lars123 0:66d33cc8318f 331 CurrentDir = Left;
lars123 0:66d33cc8318f 332 LedSet();
lars123 0:66d33cc8318f 333 dir_i = 0;
lars123 0:66d33cc8318f 334 }
lars123 0:66d33cc8318f 335 else if(dir_i >= 10 && CurrentDir == (Left || Right)){
lars123 0:66d33cc8318f 336 CurrentDir = Up;
lars123 0:66d33cc8318f 337 LedSet();
lars123 0:66d33cc8318f 338 dir_i = 0;
lars123 0:66d33cc8318f 339 }
lars123 0:66d33cc8318f 340 else if(dir_i >= 5 && CurrentDir == Left){
lars123 0:66d33cc8318f 341 CurrentDir = Right;
lars123 0:66d33cc8318f 342 LedSet();
lars123 0:66d33cc8318f 343 dir_i = 0;
lars123 0:66d33cc8318f 344 }
lars123 0:66d33cc8318f 345 else if(dir_i >= 5 && CurrentDir == Right){
lars123 0:66d33cc8318f 346 CurrentDir = Left;
lars123 0:66d33cc8318f 347 LedSet();
lars123 0:66d33cc8318f 348 dir_i = 0;
lars123 0:66d33cc8318f 349 }
lars123 0:66d33cc8318f 350 else if(dir_i >= 5 && CurrentDir == Up){
lars123 0:66d33cc8318f 351 CurrentDir = Down;
lars123 0:66d33cc8318f 352 LedSet();
lars123 0:66d33cc8318f 353 dir_i = 0;
lars123 0:66d33cc8318f 354 }
lars123 0:66d33cc8318f 355 else if(dir_i >= 5 && CurrentDir == Down){
lars123 0:66d33cc8318f 356 CurrentDir = Up;
lars123 0:66d33cc8318f 357 LedSet();
lars123 0:66d33cc8318f 358 dir_i = 0;
lars123 0:66d33cc8318f 359 }
lars123 0:66d33cc8318f 360 }
lars123 0:66d33cc8318f 361
lars123 0:66d33cc8318f 362
lars123 0:66d33cc8318f 363 }
lars123 0:66d33cc8318f 364
lars123 0:66d33cc8318f 365 void CheckValue(){ //Searches for the highest value in the emg signal up until now
lars123 0:66d33cc8318f 366 setpoint = 0;
lars123 0:66d33cc8318f 367 dir_c = 0;
lars123 0:66d33cc8318f 368 int n;
lars123 0:66d33cc8318f 369 int n1;
lars123 0:66d33cc8318f 370 for(n = 0;n<=max_n;n++){
lars123 0:66d33cc8318f 371 if(emg1_log[n] >= setpoint){
lars123 0:66d33cc8318f 372 setpoint = emg1_log[n];
lars123 0:66d33cc8318f 373 }
lars123 0:66d33cc8318f 374
lars123 0:66d33cc8318f 375 emg1_log[n] = 0;
lars123 0:66d33cc8318f 376
lars123 0:66d33cc8318f 377
lars123 0:66d33cc8318f 378
lars123 0:66d33cc8318f 379 for(n1=0;n1<=max_n;n1++){
lars123 0:66d33cc8318f 380 if(emg2_log[n1] >= dir_c){
lars123 0:66d33cc8318f 381 dir_c = emg2_log[n1];
lars123 0:66d33cc8318f 382 }
lars123 0:66d33cc8318f 383 emg2_log[n] = 0;
lars123 0:66d33cc8318f 384
lars123 0:66d33cc8318f 385 }
lars123 0:66d33cc8318f 386 }
lars123 0:66d33cc8318f 387 if(setpoint>1){ //Saturaion
lars123 0:66d33cc8318f 388 setpoint = 1;
lars123 0:66d33cc8318f 389 }
lars123 0:66d33cc8318f 390 if(setpoint<0.2){ //Rest
lars123 0:66d33cc8318f 391 setpoint = 0;
lars123 0:66d33cc8318f 392 }
lars123 0:66d33cc8318f 393 if(dir_c<0.2){ //Rest
lars123 0:66d33cc8318f 394 dir_c = 0;
lars123 0:66d33cc8318f 395 }
lars123 0:66d33cc8318f 396 }
lars123 0:66d33cc8318f 397
lars123 0:66d33cc8318f 398 void InvKin(){ //function that converts desired cartesian velocities to reference joint velocities.
lars123 0:66d33cc8318f 399 double Inverse_J[2][2] = {{(-cos(q1) + sin(q2))/((cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2))) , (L1 + L0*cos(q2) - L1*sin(q1))/(L0*(cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2)))} , {sin(q2)/(-cos(q1)*(L1 + L0*cos(q2)) + L1*sin(q1)*sin(q2)) , -((L1 + L0*cos(q2))/(L0*(cos(q1)*(L1 + L0*cos(q2)) - L1*sin(q1)*sin(q2))))}};
lars123 0:66d33cc8318f 400 switch(CurrentDir){
lars123 0:66d33cc8318f 401 case Up:
lars123 0:66d33cc8318f 402 qv[0] = 0; //Inverse_J[0][1]*setpoint;
lars123 0:66d33cc8318f 403 qv[1] = setpoint; //Inverse_J[1][1]*setpoint;
lars123 0:66d33cc8318f 404 break;
lars123 0:66d33cc8318f 405 case Down:
lars123 0:66d33cc8318f 406 qv[0] = 0; //Inverse_J[0][1]*-(setpoint);
lars123 0:66d33cc8318f 407 qv[1] = -setpoint; //Inverse_J[1][1]*-(setpoint);
lars123 0:66d33cc8318f 408 break;
lars123 0:66d33cc8318f 409 case Left:
lars123 0:66d33cc8318f 410 qv[0] = -setpoint; //Inverse_J[0][0]*-(setpoint);
lars123 0:66d33cc8318f 411 qv[1] = 0; //Inverse_J[1][0]*-(setpoint);
lars123 0:66d33cc8318f 412 break;
lars123 0:66d33cc8318f 413 case Right:
lars123 0:66d33cc8318f 414 qv[0] = setpoint; //Inverse_J[0][0]*setpoint;
lars123 0:66d33cc8318f 415 qv[1] = 0; //Inverse_J[1][0]*setpoint;
lars123 0:66d33cc8318f 416 break;
lars123 0:66d33cc8318f 417 default:
lars123 0:66d33cc8318f 418 break;
lars123 0:66d33cc8318f 419 }
lars123 0:66d33cc8318f 420 qv[0] = qv[0]*MaxV;
lars123 0:66d33cc8318f 421 qv[1] = qv[1]*MaxV;
lars123 0:66d33cc8318f 422 }
lars123 0:66d33cc8318f 423
lars123 0:66d33cc8318f 424 void CalibrateEmg(){ //function that finds the highest value in the EMG signal in order to normalise it using this value afterwards.
lars123 0:66d33cc8318f 425 MeasureBool = false;
lars123 0:66d33cc8318f 426 LedState = Off;
lars123 0:66d33cc8318f 427 int n;
lars123 0:66d33cc8318f 428 int n1;
lars123 0:66d33cc8318f 429 for(n = 0;n<=max_n;n++){
lars123 0:66d33cc8318f 430 if(emg1_log[n] >= highnum1){
lars123 0:66d33cc8318f 431 highnum1 = emg1_log[n];
lars123 0:66d33cc8318f 432 }
lars123 0:66d33cc8318f 433 emg1_log[n] = 0;
lars123 0:66d33cc8318f 434 }
lars123 0:66d33cc8318f 435 for(n1 = 0;n<=max_n;n1++){
lars123 0:66d33cc8318f 436 if(emg2_log[n1] >= highnum2){
lars123 0:66d33cc8318f 437 highnum2 = emg2_log[n1];
lars123 0:66d33cc8318f 438 }
lars123 0:66d33cc8318f 439 emg2_log[n1] = 0;
lars123 0:66d33cc8318f 440 }
lars123 0:66d33cc8318f 441 if(highnum1>1){
lars123 0:66d33cc8318f 442 highnum1 = 1;
lars123 0:66d33cc8318f 443 }
lars123 0:66d33cc8318f 444 if(highnum2>1){
lars123 0:66d33cc8318f 445 highnum2 = 1;
lars123 0:66d33cc8318f 446 }
lars123 0:66d33cc8318f 447 max_n = 0;
lars123 0:66d33cc8318f 448 /*
lars123 0:66d33cc8318f 449 if(highnum1 <= 0.2){
lars123 0:66d33cc8318f 450 pc.printf("Done calibrating \r\n%.2f Oei! Dat is jammer \r\n",highnum1);
lars123 0:66d33cc8318f 451 }
lars123 0:66d33cc8318f 452 else if(highnum1 <= 0.4){
lars123 0:66d33cc8318f 453 pc.printf("Done calibrating \r\n%.2f Kan beter he \r\n",highnum1);
lars123 0:66d33cc8318f 454 }
lars123 0:66d33cc8318f 455 else if(highnum1 <= 0.6){
lars123 0:66d33cc8318f 456 pc.printf("Done calibrating \r\n%.2f Komt in de buurt \r\n",highnum1);
lars123 0:66d33cc8318f 457 }
lars123 0:66d33cc8318f 458 else if(highnum1 <= 0.8){
lars123 0:66d33cc8318f 459 pc.printf("Done calibrating \r\n%.2f Prima! \r\n",highnum1);
lars123 0:66d33cc8318f 460 }
lars123 0:66d33cc8318f 461 else if(highnum1 <= 1){
lars123 0:66d33cc8318f 462 pc.printf("Done calibrating \r\n%.2f Lekker Bezig man!\r\n",highnum1);
lars123 0:66d33cc8318f 463 }
lars123 0:66d33cc8318f 464 */
lars123 0:66d33cc8318f 465
lars123 0:66d33cc8318f 466
lars123 0:66d33cc8318f 467 /*
lars123 0:66d33cc8318f 468 if(highnum2 <= 0.2){
lars123 0:66d33cc8318f 469 pc.printf("Done calibrating \r\n%.2f Oei! Dat is jammer \r\n",highnum2);
lars123 0:66d33cc8318f 470 }
lars123 0:66d33cc8318f 471 else if(highnum2 <= 0.4){
lars123 0:66d33cc8318f 472 pc.printf("Done calibrating \r\n%.2f Kan beter he \r\n",highnum2);
lars123 0:66d33cc8318f 473 }
lars123 0:66d33cc8318f 474 else if(highnum2 <= 0.6){
lars123 0:66d33cc8318f 475 pc.printf("Done calibrating \r\n%.2f Komt in de buurt \r\n",highnum2);
lars123 0:66d33cc8318f 476 }
lars123 0:66d33cc8318f 477 else if(highnum2 <= 0.8){
lars123 0:66d33cc8318f 478 pc.printf("Done calibrating \r\n%.2f Prima! \r\n",highnum2);
lars123 0:66d33cc8318f 479 }
lars123 0:66d33cc8318f 480 else if(highnum2 <= 1){
lars123 0:66d33cc8318f 481 pc.printf("Done calibrating \r\n%.2f Lekker Bezig man!\r\n",highnum2);
lars123 0:66d33cc8318f 482 }
lars123 0:66d33cc8318f 483 */
lars123 0:66d33cc8318f 484 CalibratedE = true;
lars123 0:66d33cc8318f 485 MeasureBool = true;
lars123 0:66d33cc8318f 486 wait(0.1f);
lars123 0:66d33cc8318f 487 pause_loop = false;
lars123 0:66d33cc8318f 488 }
lars123 0:66d33cc8318f 489
lars123 0:66d33cc8318f 490 void CalibrateMotors(){ //funtion used to calibrate the motors to the reference position after moving towards the mechanical limit.
lars123 0:66d33cc8318f 491 static double q1_prev = q1;
lars123 0:66d33cc8318f 492 static double q2_prev = q2;
lars123 0:66d33cc8318f 493 /*
lars123 0:66d33cc8318f 494 if(CalibratedM){
lars123 0:66d33cc8318f 495 pc.printf("\r\n %i %f %f %f %f %i",Motor2DIR.read(),Motor1PWR.read(), Motor2PWR.read(),q2,qr[1],CalibratedM);
lars123 0:66d33cc8318f 496 wait(0.05);
lars123 0:66d33cc8318f 497 }
lars123 0:66d33cc8318f 498 */
lars123 0:66d33cc8318f 499 //pc.printf("\r\n%f",q1);
lars123 0:66d33cc8318f 500 MeasurePos();
lars123 0:66d33cc8318f 501 if((((fabs(q1_prev-q1)<0.02 && fabs(q2_prev-q2)<0.02) && Homing == false) && Cteller1 >= 2) && !CalibratedM){
lars123 0:66d33cc8318f 502 if(Cteller>=3){
lars123 0:66d33cc8318f 503 Motor1PWR.write(0);
lars123 0:66d33cc8318f 504 Motor2PWR.write(0);
lars123 0:66d33cc8318f 505 counts1 = 0;
lars123 0:66d33cc8318f 506 counts2 = 0;
lars123 0:66d33cc8318f 507 Encoder1.reset();
lars123 0:66d33cc8318f 508 Encoder2.reset();
lars123 0:66d33cc8318f 509 q1 = 0;
lars123 0:66d33cc8318f 510 q2 = 0;
lars123 0:66d33cc8318f 511 //pc.printf("\r\n%i",counts1);
lars123 0:66d33cc8318f 512 Homing = true;
lars123 0:66d33cc8318f 513 Cteller = 0;
lars123 0:66d33cc8318f 514 }
lars123 0:66d33cc8318f 515 Cteller++;
lars123 0:66d33cc8318f 516 Cteller1 = 0;
lars123 0:66d33cc8318f 517 }
lars123 0:66d33cc8318f 518 else if(Homing){
lars123 0:66d33cc8318f 519 logcq = true;
lars123 0:66d33cc8318f 520 qr[0] = refpos1;//-1.3962634;
lars123 0:66d33cc8318f 521 qr[1] = refpos2;
lars123 0:66d33cc8318f 522 //pc.printf("\r\n %i %f %f %f %i",Motor2DIR.read(),Motor2PWR.read(),q2,qr[1],CalibratedM);
lars123 0:66d33cc8318f 523 //wait(0.05);
lars123 0:66d33cc8318f 524 if(((fabs(qr[0]-q1)<0.02) && Cteller >=3) && (fabs(qr[1]-q2)<0.02)){
lars123 0:66d33cc8318f 525 //pc.printf("\r\n Calibrated\r\n\r\n");
lars123 0:66d33cc8318f 526 CalibratedM = true;
lars123 0:66d33cc8318f 527 Homing = false;
lars123 0:66d33cc8318f 528 Cteller = 0;
lars123 0:66d33cc8318f 529 }
lars123 0:66d33cc8318f 530 else if(((fabs(qr[0]-q1)<0.02) && (fabs(qr[1]-q2)<0.02)) && !CalibratedM){
lars123 0:66d33cc8318f 531 Cteller++;
lars123 0:66d33cc8318f 532 }
lars123 0:66d33cc8318f 533 }
lars123 0:66d33cc8318f 534 if(CalibratedM){
lars123 0:66d33cc8318f 535 if(Cteller == 0){
lars123 0:66d33cc8318f 536 //pc.printf("\r\n Calibrated\r\n\r\n");
lars123 0:66d33cc8318f 537 //wait(0.05f);
lars123 0:66d33cc8318f 538 /*
lars123 0:66d33cc8318f 539 int n;
lars123 0:66d33cc8318f 540 for(n = 0; n <= max_n;n++){
lars123 0:66d33cc8318f 541 pc.printf(" %f,",cqlog[n]);
lars123 0:66d33cc8318f 542 wait(0.01f);
lars123 0:66d33cc8318f 543 }
lars123 0:66d33cc8318f 544 */
lars123 0:66d33cc8318f 545 max_n = 0;
lars123 0:66d33cc8318f 546
lars123 0:66d33cc8318f 547 Motor1PWR.write(0);
lars123 0:66d33cc8318f 548 Motor2PWR.write(0);
lars123 0:66d33cc8318f 549 counts1 = 0;
lars123 0:66d33cc8318f 550 counts2 = 0;
lars123 0:66d33cc8318f 551 Encoder1.reset();
lars123 0:66d33cc8318f 552 Encoder2.reset();
lars123 0:66d33cc8318f 553 q1 = pi/4;
lars123 0:66d33cc8318f 554 q2 = pi/4;
lars123 0:66d33cc8318f 555 qr[0] = pi/4;
lars123 0:66d33cc8318f 556 qr[1] = pi/4;
lars123 0:66d33cc8318f 557
lars123 0:66d33cc8318f 558 pause_loop = false;
lars123 0:66d33cc8318f 559 }
lars123 0:66d33cc8318f 560 Cteller++;
lars123 0:66d33cc8318f 561 //pause_loop = false;
lars123 0:66d33cc8318f 562 }
lars123 0:66d33cc8318f 563 Cteller1 ++;
lars123 0:66d33cc8318f 564 q1_prev = q1;
lars123 0:66d33cc8318f 565 q2_prev = q2;
lars123 0:66d33cc8318f 566 }
lars123 0:66d33cc8318f 567
lars123 0:66d33cc8318f 568 double PID_Controller(double error, int n){ //this function performs the task of PID controller for both motors
lars123 0:66d33cc8318f 569 double fout = 0;
lars123 0:66d33cc8318f 570 if(n == 1){
lars123 0:66d33cc8318f 571 static double error_integral1 = 0;
lars123 0:66d33cc8318f 572 static double error_prev1 = error; // initialization with this value only done once!
lars123 0:66d33cc8318f 573 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
lars123 0:66d33cc8318f 574
lars123 0:66d33cc8318f 575 // Proportional part:
lars123 0:66d33cc8318f 576 double u_k1 = Kp1 * error;
lars123 0:66d33cc8318f 577
lars123 0:66d33cc8318f 578 // Integral part
lars123 0:66d33cc8318f 579 error_integral1 = error_integral1 + error * Ts;
lars123 0:66d33cc8318f 580 double u_i1 = Ki1 * error_integral1;
lars123 0:66d33cc8318f 581
lars123 0:66d33cc8318f 582 // Derivative part
lars123 0:66d33cc8318f 583 double error_derivative1 = (error - error_prev1)/Ts;
lars123 0:66d33cc8318f 584 double filtered_error_derivative1 = LowPassFilter.step(error_derivative1);
lars123 0:66d33cc8318f 585 double u_d1 = Kd1 * filtered_error_derivative1;
lars123 0:66d33cc8318f 586 error_prev1 = error;
lars123 0:66d33cc8318f 587
lars123 0:66d33cc8318f 588 fout = u_k1 + u_i1 + u_d1;
lars123 0:66d33cc8318f 589
lars123 0:66d33cc8318f 590 }
lars123 0:66d33cc8318f 591
lars123 0:66d33cc8318f 592 if(n == 2){
lars123 0:66d33cc8318f 593 static double error_integral2 = 0;
lars123 0:66d33cc8318f 594 static double error_prev2 = error; // initialization with this value only done once!
lars123 0:66d33cc8318f 595 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
lars123 0:66d33cc8318f 596
lars123 0:66d33cc8318f 597 // Proportional part:
lars123 0:66d33cc8318f 598 double u_k2 = Kp2 * error;
lars123 0:66d33cc8318f 599
lars123 0:66d33cc8318f 600 // Integral part
lars123 0:66d33cc8318f 601 error_integral2 = error_integral2 + error * Ts;
lars123 0:66d33cc8318f 602 double u_i2 = Ki2 * error_integral2;
lars123 0:66d33cc8318f 603
lars123 0:66d33cc8318f 604 // Derivative part
lars123 0:66d33cc8318f 605 double error_derivative2 = (error - error_prev2)/Ts;
lars123 0:66d33cc8318f 606 double filtered_error_derivative2 = LowPassFilter.step(error_derivative2);
lars123 0:66d33cc8318f 607 double u_d2 = Kd2 * filtered_error_derivative2;
lars123 0:66d33cc8318f 608 error_prev2 = error;
lars123 0:66d33cc8318f 609
lars123 0:66d33cc8318f 610 // Sum all parts and return it
lars123 0:66d33cc8318f 611 fout = u_k2 + u_i2 + u_d2;
lars123 0:66d33cc8318f 612 }
lars123 0:66d33cc8318f 613
lars123 0:66d33cc8318f 614 // Sum all parts and return it
lars123 0:66d33cc8318f 615 if (fabs(error)>=0.01){
lars123 0:66d33cc8318f 616 return fout;
lars123 0:66d33cc8318f 617 }
lars123 0:66d33cc8318f 618 else{
lars123 0:66d33cc8318f 619 return 0;
lars123 0:66d33cc8318f 620 }
lars123 0:66d33cc8318f 621 }
lars123 0:66d33cc8318f 622
lars123 0:66d33cc8318f 623 void LogSignals(){ //sends a log of th signal to the pc via the serial port
lars123 0:66d33cc8318f 624 MeasureBool = false;
lars123 0:66d33cc8318f 625 int n;
lars123 0:66d33cc8318f 626 for(n = 0;n<=max_n;n++){
lars123 0:66d33cc8318f 627 emg1_log[n] = emg1_log[n]/highnum1;
lars123 0:66d33cc8318f 628 //pc.printf("%f, ",emg1_log[n]);
lars123 0:66d33cc8318f 629 //wait(0.01f);
lars123 0:66d33cc8318f 630 }
lars123 0:66d33cc8318f 631 pc.printf("\r\n\r\n%i", max_n);
lars123 0:66d33cc8318f 632 ClearEmg();
lars123 0:66d33cc8318f 633 pause_loop = false;
lars123 0:66d33cc8318f 634 wait(0.1f);
lars123 0:66d33cc8318f 635 }
lars123 0:66d33cc8318f 636
lars123 0:66d33cc8318f 637 void CheckDIR1(){ //new function used to check the direction that the arm should move in.
lars123 0:66d33cc8318f 638 if(button1.read() == 0){
lars123 0:66d33cc8318f 639 if(CurrentDir == Up){
lars123 0:66d33cc8318f 640 CurrentDir = Down;
lars123 0:66d33cc8318f 641 LedSet();
lars123 0:66d33cc8318f 642 }
lars123 0:66d33cc8318f 643 else if(CurrentDir == Down){
lars123 0:66d33cc8318f 644 CurrentDir = Left;
lars123 0:66d33cc8318f 645 LedSet();
lars123 0:66d33cc8318f 646 }
lars123 0:66d33cc8318f 647 else if(CurrentDir == Left){
lars123 0:66d33cc8318f 648 CurrentDir = Right;
lars123 0:66d33cc8318f 649 LedSet();
lars123 0:66d33cc8318f 650 }
lars123 0:66d33cc8318f 651 else if(CurrentDir == Right){
lars123 0:66d33cc8318f 652 CurrentDir = Up;
lars123 0:66d33cc8318f 653 LedSet();
lars123 0:66d33cc8318f 654 }
lars123 0:66d33cc8318f 655 }
lars123 0:66d33cc8318f 656 }
lars123 0:66d33cc8318f 657
lars123 0:66d33cc8318f 658 void SetPointJoint(){ //sets the desirdered cartesian velocity depending on the highest measured value in the array up untill now array is cleared afterwards.
lars123 0:66d33cc8318f 659 if(MoveBool){
lars123 0:66d33cc8318f 660 MeasureBool = false;
lars123 0:66d33cc8318f 661 CheckDIR1();
lars123 0:66d33cc8318f 662 if(button2.read() == 0){
lars123 0:66d33cc8318f 663 setpoint = 0.2;
lars123 0:66d33cc8318f 664 }
lars123 0:66d33cc8318f 665 else{
lars123 0:66d33cc8318f 666 setpoint = 0;
lars123 0:66d33cc8318f 667 }
lars123 0:66d33cc8318f 668
lars123 0:66d33cc8318f 669 //CheckValue();
lars123 0:66d33cc8318f 670 //pc.printf("\r\n%f %f",q1,q2);
lars123 0:66d33cc8318f 671 //wait(0.05f);
lars123 0:66d33cc8318f 672 ClearEmg();
lars123 0:66d33cc8318f 673 MeasureBool = true;
lars123 0:66d33cc8318f 674 }
lars123 0:66d33cc8318f 675 }
lars123 0:66d33cc8318f 676
lars123 0:66d33cc8318f 677
lars123 0:66d33cc8318f 678
lars123 0:66d33cc8318f 679
lars123 0:66d33cc8318f 680 void Movemotors(double input1, double input2){ //function to move the two motors given two inputs
lars123 0:66d33cc8318f 681 if(input1>0){
lars123 0:66d33cc8318f 682 Motor1DIR.write(0);
lars123 0:66d33cc8318f 683 }
lars123 0:66d33cc8318f 684 else{
lars123 0:66d33cc8318f 685 Motor1DIR.write(1);
lars123 0:66d33cc8318f 686 }
lars123 0:66d33cc8318f 687 if(fabs(input1)>1){
lars123 0:66d33cc8318f 688 Motor1PWR.write(1);
lars123 0:66d33cc8318f 689 }
lars123 0:66d33cc8318f 690 else{
lars123 0:66d33cc8318f 691 Motor1PWR.write(fabs(input1));
lars123 0:66d33cc8318f 692 }
lars123 0:66d33cc8318f 693 if(input2>0){
lars123 0:66d33cc8318f 694 Motor2DIR.write(0);
lars123 0:66d33cc8318f 695 }
lars123 0:66d33cc8318f 696 else{
lars123 0:66d33cc8318f 697 Motor2DIR.write(1);
lars123 0:66d33cc8318f 698 }
lars123 0:66d33cc8318f 699 if(fabs(input2)>1){
lars123 0:66d33cc8318f 700 Motor2PWR.write(1);
lars123 0:66d33cc8318f 701 }
lars123 0:66d33cc8318f 702 else{
lars123 0:66d33cc8318f 703 Motor2PWR.write(fabs(input2));
lars123 0:66d33cc8318f 704 }
lars123 0:66d33cc8318f 705 }
lars123 0:66d33cc8318f 706
lars123 0:66d33cc8318f 707 void MeasureAndControl(){ //function that serves as the control loop operating at 1000Hz. It compares the current position to the desired position and feeds this error to the PID. Then the PID sends an output to the motors
lars123 0:66d33cc8318f 708 if(Homing){
lars123 0:66d33cc8318f 709 InvKin();
lars123 0:66d33cc8318f 710 ReferencePos();
lars123 0:66d33cc8318f 711 MeasurePos();
lars123 0:66d33cc8318f 712 if((logcq && Cnum >= 10) && !CalibratedM){
lars123 0:66d33cc8318f 713 cqlog[max_n] = q2;
lars123 0:66d33cc8318f 714 max_n++;
lars123 0:66d33cc8318f 715 Cnum = 0;
lars123 0:66d33cc8318f 716 }
lars123 0:66d33cc8318f 717 double motorinput1 = PID_Controller(qr[0] - q1,1);
lars123 0:66d33cc8318f 718 double motorinput2 = PID_Controller(qr[1] - q2,2);
lars123 0:66d33cc8318f 719 if(motorinput1> 0.1){
lars123 0:66d33cc8318f 720 motorinput1 += 0.45;
lars123 0:66d33cc8318f 721 }
lars123 0:66d33cc8318f 722 else if(motorinput1 < 0.1){
lars123 0:66d33cc8318f 723 motorinput1 -= 0.45;
lars123 0:66d33cc8318f 724 }
lars123 0:66d33cc8318f 725 if(motorinput2>0.1){
lars123 0:66d33cc8318f 726 motorinput2 += 0.1;
lars123 0:66d33cc8318f 727 }
lars123 0:66d33cc8318f 728 else if(motorinput2 < -0.1){
lars123 0:66d33cc8318f 729 motorinput2 -= 0.1;
lars123 0:66d33cc8318f 730 }
lars123 0:66d33cc8318f 731 Movemotors(motorinput1,motorinput2);
lars123 0:66d33cc8318f 732 Cnum ++;
lars123 0:66d33cc8318f 733 }
lars123 0:66d33cc8318f 734 if(CalibratedM){
lars123 0:66d33cc8318f 735 InvKin();
lars123 0:66d33cc8318f 736 ReferencePos();
lars123 0:66d33cc8318f 737 MeasurePos();
lars123 0:66d33cc8318f 738 double motorinput1 = PID_Controller(qr[0] - q1,1);
lars123 0:66d33cc8318f 739 double motorinput2 = PID_Controller(qr[1] - q2,2);
lars123 0:66d33cc8318f 740 if(motorinput1> 0.05){
lars123 0:66d33cc8318f 741 motorinput1 += 0.45;
lars123 0:66d33cc8318f 742 }
lars123 0:66d33cc8318f 743 else if(motorinput1 < 0.05){
lars123 0:66d33cc8318f 744 motorinput1 -= 0.45;
lars123 0:66d33cc8318f 745 }
lars123 0:66d33cc8318f 746 if(motorinput2>0.05){
lars123 0:66d33cc8318f 747 motorinput2 += 045;
lars123 0:66d33cc8318f 748 }
lars123 0:66d33cc8318f 749 else if(motorinput2 < -0.05){
lars123 0:66d33cc8318f 750 motorinput2 -= 0.45;
lars123 0:66d33cc8318f 751 }
lars123 0:66d33cc8318f 752 Movemotors(motorinput1,motorinput2);
lars123 0:66d33cc8318f 753 }
lars123 0:66d33cc8318f 754 }
lars123 0:66d33cc8318f 755
lars123 0:66d33cc8318f 756 void PIDTuning(){ //function that was used to tune the PID using trial and error methods
lars123 0:66d33cc8318f 757 if(Pot1*4<1){
lars123 0:66d33cc8318f 758 Kp2 += button1*0.1;
lars123 0:66d33cc8318f 759 Kp2-= button2*0.1;
lars123 0:66d33cc8318f 760 }
lars123 0:66d33cc8318f 761 else if(Pot1*4<2){
lars123 0:66d33cc8318f 762 Ki2+= button1*0.1;
lars123 0:66d33cc8318f 763 Ki2-= button2*0.1;
lars123 0:66d33cc8318f 764 }
lars123 0:66d33cc8318f 765 else if(Pot1*4<3){
lars123 0:66d33cc8318f 766 Kd2 += button1*0.1;
lars123 0:66d33cc8318f 767 Kd2 -= button2*0.1;
lars123 0:66d33cc8318f 768 }
lars123 0:66d33cc8318f 769 else if(Pot1*4<4){
lars123 0:66d33cc8318f 770 refpos2 += button1*0.1;
lars123 0:66d33cc8318f 771 refpos2 -= button2*0.1;
lars123 0:66d33cc8318f 772 }
lars123 0:66d33cc8318f 773 if(Cteller == 5){
lars123 0:66d33cc8318f 774 printf("\r\n %f %f %f %f %f",Kp2,Ki2,Kd2,refpos2,Pot1.read()*4);
lars123 0:66d33cc8318f 775 Cteller = 0;
lars123 0:66d33cc8318f 776 }
lars123 0:66d33cc8318f 777 if(Pot2>0.8f){
lars123 0:66d33cc8318f 778 pause_loop = false;
lars123 0:66d33cc8318f 779 }
lars123 0:66d33cc8318f 780 Cteller++;
lars123 0:66d33cc8318f 781 }
lars123 0:66d33cc8318f 782
lars123 0:66d33cc8318f 783 void StirU(){ // function that was used to stir and move the stirring servo up
lars123 0:66d33cc8318f 784 if(stirred == true){
lars123 0:66d33cc8318f 785 StirDown.write(0);
lars123 0:66d33cc8318f 786 StirUp.write(1);
lars123 0:66d33cc8318f 787 pc.printf("Ho");
lars123 0:66d33cc8318f 788 wait(0.05f);
lars123 0:66d33cc8318f 789 pause_loop = false;
lars123 0:66d33cc8318f 790 }
lars123 0:66d33cc8318f 791 }
lars123 0:66d33cc8318f 792
lars123 0:66d33cc8318f 793 void StirD(){ //used to move the stirring servo down
lars123 0:66d33cc8318f 794 if(button1.read() == 0){
lars123 0:66d33cc8318f 795 StirDown.write(1);
lars123 0:66d33cc8318f 796 StirUp.write(0);
lars123 0:66d33cc8318f 797 }
lars123 0:66d33cc8318f 798
lars123 0:66d33cc8318f 799 }
lars123 0:66d33cc8318f 800
lars123 0:66d33cc8318f 801
lars123 0:66d33cc8318f 802 void CancelStir(){ //used to cancel the stirring operation.
lars123 0:66d33cc8318f 803 StirDown.write(0);
lars123 0:66d33cc8318f 804 StirUp.write(1);
lars123 0:66d33cc8318f 805 pause_loop = false;
lars123 0:66d33cc8318f 806 stirred = false;
lars123 0:66d33cc8318f 807 pc.printf("hee");
lars123 0:66d33cc8318f 808 wait(0.05f);
lars123 0:66d33cc8318f 809 }
lars123 0:66d33cc8318f 810
lars123 0:66d33cc8318f 811 void SwitchState(){ //switches to the coffee state
lars123 0:66d33cc8318f 812 if((button1.read() == 0) && (button2.read() == 0)){
lars123 0:66d33cc8318f 813 pause_loop = false;
lars123 0:66d33cc8318f 814 }
lars123 0:66d33cc8318f 815 }
lars123 0:66d33cc8318f 816
lars123 0:66d33cc8318f 817 void ProcessStateMachine(){ //main statemachine
lars123 0:66d33cc8318f 818 switch(ProgramState){
lars123 0:66d33cc8318f 819 case CalibrateM:
lars123 0:66d33cc8318f 820 ScopeTimer.attach(&PIDTuning,0.1); //tune PID
lars123 0:66d33cc8318f 821 while(pause_loop){}
lars123 0:66d33cc8318f 822 if(start){
lars123 0:66d33cc8318f 823 Motor1DIR.write(1); //moving to mechainical limit
lars123 0:66d33cc8318f 824 Motor2DIR.write(1);
lars123 0:66d33cc8318f 825 Motor1PWR.write(0.55);
lars123 0:66d33cc8318f 826 Motor2PWR.write(0.6);
lars123 0:66d33cc8318f 827 start = false;
lars123 0:66d33cc8318f 828 }
lars123 0:66d33cc8318f 829 pause_loop = true;
lars123 0:66d33cc8318f 830 //BlinkBool = true;
lars123 0:66d33cc8318f 831 LedState = Red;
lars123 0:66d33cc8318f 832 LedBlink(); //change LED
lars123 0:66d33cc8318f 833 ScopeTimer.attach(&CalibrateMotors,0.3); //move to reference position
lars123 0:66d33cc8318f 834 LedTimer.attach(MeasureAndControl,1/fcontrol); //control loop
lars123 0:66d33cc8318f 835
lars123 0:66d33cc8318f 836 while(pause_loop){}
lars123 0:66d33cc8318f 837 pause_loop = true;
lars123 0:66d33cc8318f 838 if(CalibratedM){
lars123 0:66d33cc8318f 839 ProgramState = MandM; // go to measure and move STATE
lars123 0:66d33cc8318f 840 }
lars123 0:66d33cc8318f 841 break;
lars123 0:66d33cc8318f 842 case TestStand: //auxiliary state
lars123 0:66d33cc8318f 843 LedState = Purple;
lars123 0:66d33cc8318f 844 ScopeTimer.attach(&SetPointJoint,0.2);
lars123 0:66d33cc8318f 845 ControlTicker.attach(&LedBlink,0.2);
lars123 0:66d33cc8318f 846 while(pause_loop){}
lars123 0:66d33cc8318f 847
lars123 0:66d33cc8318f 848 break;
lars123 0:66d33cc8318f 849 case CalibrateE: //calibrate EMG signals
lars123 0:66d33cc8318f 850 LedState = Green;
lars123 0:66d33cc8318f 851 pause_loop = true;
lars123 0:66d33cc8318f 852 LedTimer.attach(&LedBlink,0.5);
lars123 0:66d33cc8318f 853 ScopeTimer.attach(&EmgMeasure,1/fsample);
lars123 0:66d33cc8318f 854 //MakeEmg();
lars123 0:66d33cc8318f 855 SignalTimeout.attach(&CalibrateEmg,5);
lars123 0:66d33cc8318f 856 while(pause_loop){}
lars123 0:66d33cc8318f 857 ProgramState = MandM;
lars123 0:66d33cc8318f 858 break;
lars123 0:66d33cc8318f 859 case MandM: //measure and move state.
lars123 0:66d33cc8318f 860 LedState = Off;
lars123 0:66d33cc8318f 861 pause_loop = true;
lars123 0:66d33cc8318f 862 MoveBool = true;
lars123 0:66d33cc8318f 863 LedTimer.attach(&LedBlink,0.2); //blinks
lars123 0:66d33cc8318f 864 ControlTicker.attach(&SwitchState,0.3); //if buttons are both pressed switches to coffee state
lars123 0:66d33cc8318f 865 //SignalTimeout.attach(CancelPause,0.5);
lars123 0:66d33cc8318f 866 //SignalTimeout.attach(&LogSignals,5);
lars123 0:66d33cc8318f 867 ScopeTimer.attach(&SetPointJoint,0.5); //determines the setpoint joint velocity. based on button presses. in full design based on EMG1 input
lars123 0:66d33cc8318f 868 ControlTicker1.attach(&MeasureAndControl,1/fcontrol); //the 100HZ control loop
lars123 0:66d33cc8318f 869 while(pause_loop){}
lars123 0:66d33cc8318f 870 ProgramState = Coffee; //go to coffee state
lars123 0:66d33cc8318f 871 break;
lars123 0:66d33cc8318f 872 case Coffee:
lars123 0:66d33cc8318f 873 LedState = Purple;
lars123 0:66d33cc8318f 874 LedBlink();
lars123 0:66d33cc8318f 875 pause_loop = true;
lars123 0:66d33cc8318f 876 LedTimer.attach(&StirD,0.2); //stir down
lars123 0:66d33cc8318f 877 SignalTimeout.attach(&CancelStir,3); //after 3 seconds stir up
lars123 0:66d33cc8318f 878 while(pause_loop){}
lars123 0:66d33cc8318f 879 ProgramState = MandM; // go to measure and move state
lars123 0:66d33cc8318f 880 break;
lars123 0:66d33cc8318f 881 default: //state if program is done
lars123 0:66d33cc8318f 882 LedState = Off;
lars123 0:66d33cc8318f 883 if(statechanged){
lars123 0:66d33cc8318f 884 pc.printf("\r\nyeeee\r\n");
lars123 0:66d33cc8318f 885 statechanged = false;
lars123 0:66d33cc8318f 886 }
lars123 0:66d33cc8318f 887 break;
lars123 0:66d33cc8318f 888 }
lars123 0:66d33cc8318f 889 }
lars123 0:66d33cc8318f 890
lars123 0:66d33cc8318f 891 int main(){
lars123 0:66d33cc8318f 892 led1.write(1); //at the start all LEDs are off
lars123 0:66d33cc8318f 893 led2.write(1);
lars123 0:66d33cc8318f 894 led3.write(1);
lars123 0:66d33cc8318f 895 StirUp.write(0);
lars123 0:66d33cc8318f 896 StirDown.write(0); //All servos off
lars123 0:66d33cc8318f 897
lars123 0:66d33cc8318f 898 pc.baud(115200); // set baud rate
lars123 0:66d33cc8318f 899 Motor1PWR.period_us(60); // set PWM period
lars123 0:66d33cc8318f 900 Motor2PWR.period_us(60);
lars123 0:66d33cc8318f 901 bqc.add( &bq1).add( &bq2 ).add( &bq3 ).add( &bq4 ).add( &bq5 ); //make filter cascade
lars123 0:66d33cc8318f 902
lars123 0:66d33cc8318f 903 while(1){
lars123 0:66d33cc8318f 904 ProcessStateMachine(); //execute statemachine.
lars123 0:66d33cc8318f 905 }
lars123 0:66d33cc8318f 906 }