Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

main.cpp@6:aa62e6650559, 2016-10-31 (annotated)

Committer:
s1588141
Date:
Mon Oct 31 11:16:29 2016 +0000
Revision:
6:aa62e6650559
Parent:
5:4b2ff2a4664a
Child:
7:075ba23f3147
EMG not working after implementing 3rd biquad

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1588141 0:a2db8cc5d5df 1 #include "mbed.h"
s1588141 0:a2db8cc5d5df 2 #include "MODSERIAL.h"
s1588141 4:8b1df22779a7 3 #include "QEI.h"
s1588141 2:0954eb04bb4c 4 #include "math.h"
s1588141 2:0954eb04bb4c 5 #include "HIDScope.h"
s1588141 5:4b2ff2a4664a 6 #include "BiQuad.h"
s1588141 2:0954eb04bb4c 7
s1588141 4:8b1df22779a7 8
s1588141 4:8b1df22779a7 9 ///////////////////////////
s1588141 4:8b1df22779a7 10 ////////Constantes/////////
s1588141 4:8b1df22779a7 11 ///////////////////////////
s1588141 4:8b1df22779a7 12
s1588141 4:8b1df22779a7 13 double const pi = 3.14159265359;
s1588141 4:8b1df22779a7 14
s1588141 0:a2db8cc5d5df 15 ///////////////////////////
s1588141 4:8b1df22779a7 16 // Initialise hardware/////
s1588141 0:a2db8cc5d5df 17 ///////////////////////////
s1588141 4:8b1df22779a7 18
s1588141 4:8b1df22779a7 19 MODSERIAL pc(USBTX, USBRX);
s1588141 4:8b1df22779a7 20 //-----------------------------------------------------------
s1588141 4:8b1df22779a7 21 //--------------------All lights-----------------------------
s1588141 4:8b1df22779a7 22 //-----------------------------------------------------------
s1588141 4:8b1df22779a7 23 DigitalOut green(LED_GREEN);
s1588141 4:8b1df22779a7 24 DigitalOut red(LED_RED);
s1588141 0:a2db8cc5d5df 25 //------------------------------------------------------------
s1588141 2:0954eb04bb4c 26 //--------------------All sensors-----------------------------
s1588141 0:a2db8cc5d5df 27 //------------------------------------------------------------
s1588141 4:8b1df22779a7 28
s1588141 2:0954eb04bb4c 29 //--------------------Encoders--------------------------------
s1588141 0:a2db8cc5d5df 30
s1588141 4:8b1df22779a7 31 //Variables
s1588141 4:8b1df22779a7 32
s1588141 5:4b2ff2a4664a 33 volatile double Signal = 0.0;
s1588141 4:8b1df22779a7 34 const int SampleFrequency = 100;
s1588141 4:8b1df22779a7 35 const double AnglePerPulse = 2*pi/4200; //Soms doet dit een beetje vaag, dus dit moet wel af en toe uitgetest worden
s1588141 4:8b1df22779a7 36 volatile double Position_L = 0.0; //The position of the left motor in radiants
s1588141 4:8b1df22779a7 37 volatile double Position_R = 0.0; //The position of the right motor in radiants
s1588141 4:8b1df22779a7 38 volatile double Previous_Position_L =0.0; //The previous position of the left motor in radiants
s1588141 4:8b1df22779a7 39 volatile double Previous_Position_R = 0.0; //The previous position of the right motor in radiants
s1588141 4:8b1df22779a7 40 volatile double Speed_L = 0.0; //The speed of the left motor
s1588141 4:8b1df22779a7 41 volatile double Speed_R = 0.0; //The speed of the right motor
s1588141 0:a2db8cc5d5df 42
s1588141 4:8b1df22779a7 43 //Defining Pins
s1588141 4:8b1df22779a7 44 QEI Encoder_L (D11, D10,NC, 64); //D11 is poort A D10 is poort b
s1588141 4:8b1df22779a7 45 QEI Encoder_R (D13, D12,NC, 64); //D 13 is poort A 12 is poort b
s1588141 4:8b1df22779a7 46
s1588141 4:8b1df22779a7 47 //--------------------AnalogInput---------------------------------
s1588141 5:4b2ff2a4664a 48 AnalogIn Left(A0); //Motorspeed control Left
s1588141 5:4b2ff2a4664a 49 AnalogIn Right(A1); //MotorSpeed control Right
s1588141 5:4b2ff2a4664a 50 AnalogIn Change(A2);//EMG signal for other controls
s1588141 5:4b2ff2a4664a 51 double EMG_L;
s1588141 5:4b2ff2a4664a 52 double EMG_R;
s1588141 5:4b2ff2a4664a 53 double EMG_Change;
s1588141 2:0954eb04bb4c 54 //-------------------Hidscope----------------------------------
s1588141 4:8b1df22779a7 55 HIDScope scope(4); // Sending two sets of data
s1588141 0:a2db8cc5d5df 56
s1588141 4:8b1df22779a7 57 //--------------------------------------------------------------
s1588141 4:8b1df22779a7 58 //--------------------All Motors--------------------------------
s1588141 4:8b1df22779a7 59 //--------------------------------------------------------------
s1588141 5:4b2ff2a4664a 60 volatile int movement = 0,direction_L =0, direction_R =0; //determining the direction of the motors
s1588141 4:8b1df22779a7 61
s1588141 4:8b1df22779a7 62 DigitalOut M_L_D(D4); //Richting motor links->
s1588141 4:8b1df22779a7 63 //while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive
s1588141 2:0954eb04bb4c 64 PwmOut M_L_S(D5); //Speed motor links
s1588141 0:a2db8cc5d5df 65 DigitalOut M_R_D(D7); //Richting motor Rechts
s1588141 2:0954eb04bb4c 66 PwmOut M_R_S(D6); //Speed motor Rechts
s1588141 4:8b1df22779a7 67 const int Motor_Frequency = 1000;
s1588141 4:8b1df22779a7 68
s1588141 4:8b1df22779a7 69 DigitalOut Grap(D9); //Graple active or not;
s1588141 4:8b1df22779a7 70
s1588141 4:8b1df22779a7 71 //----------------------Lights ---------------------------------4
s1588141 4:8b1df22779a7 72 DigitalOut translation(D0);
s1588141 4:8b1df22779a7 73 DigitalOut grip1(D1);
s1588141 4:8b1df22779a7 74 DigitalOut rotation(D2);
s1588141 4:8b1df22779a7 75 DigitalOut grip2(D3);
s1588141 0:a2db8cc5d5df 76
s1588141 0:a2db8cc5d5df 77 //--------------------------------------------------------------
s1588141 0:a2db8cc5d5df 78 //-----------------------Functions------------------------------
s1588141 0:a2db8cc5d5df 79 //--------------------------------------------------------------
s1588141 0:a2db8cc5d5df 80
s1588141 4:8b1df22779a7 81 //-----------------------on of sitch of the sytem-----------------------------
s1588141 4:8b1df22779a7 82
s1588141 5:4b2ff2a4664a 83 InterruptIn OnOff(SW3); //System On/off
s1588141 1:d8bb72c9c019 84 volatile bool Active = false;
s1588141 4:8b1df22779a7 85 void Start_Stop(){
s1588141 4:8b1df22779a7 86 Active = !Active;
s1588141 4:8b1df22779a7 87 }
s1588141 4:8b1df22779a7 88
s1588141 4:8b1df22779a7 89
s1588141 4:8b1df22779a7 90 //--------------------------------Sampling------------------------------------------
s1588141 5:4b2ff2a4664a 91
s1588141 5:4b2ff2a4664a 92 Ticker Tick_Sample;// ticker for data sampling
s1588141 5:4b2ff2a4664a 93
s1588141 4:8b1df22779a7 94 bool Sample_Flag = false;
s1588141 4:8b1df22779a7 95
s1588141 4:8b1df22779a7 96 void Set_Sample_Flag(){
s1588141 4:8b1df22779a7 97 Sample_Flag = true;
s1588141 0:a2db8cc5d5df 98 }
s1588141 0:a2db8cc5d5df 99
s1588141 4:8b1df22779a7 100 void Sample(){
s1588141 1:d8bb72c9c019 101
s1588141 4:8b1df22779a7 102 //This function reads out the position and the speed of the motors in radiants
s1588141 4:8b1df22779a7 103 //Aand stores them in the values Speed and Position
s1588141 4:8b1df22779a7 104
s1588141 2:0954eb04bb4c 105 //Position in Radials:
s1588141 2:0954eb04bb4c 106 Previous_Position_L = Position_L;
s1588141 2:0954eb04bb4c 107 Previous_Position_R = Position_R;
s1588141 2:0954eb04bb4c 108 Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
s1588141 2:0954eb04bb4c 109 Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
s1588141 2:0954eb04bb4c 110
s1588141 2:0954eb04bb4c 111 //Speed in RadPers second
s1588141 4:8b1df22779a7 112 Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
s1588141 4:8b1df22779a7 113 Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
s1588141 4:8b1df22779a7 114
s1588141 4:8b1df22779a7 115 Encoder_L.reset();
s1588141 4:8b1df22779a7 116 Encoder_R.reset();
s1588141 4:8b1df22779a7 117
s1588141 4:8b1df22779a7 118 }
s1588141 4:8b1df22779a7 119
s1588141 4:8b1df22779a7 120 bool Controller_Flag = false;
s1588141 4:8b1df22779a7 121
s1588141 4:8b1df22779a7 122 Ticker Tick_Controller;
s1588141 4:8b1df22779a7 123
s1588141 4:8b1df22779a7 124 void Set_controller_Flag(){
s1588141 4:8b1df22779a7 125 Controller_Flag = true;
s1588141 4:8b1df22779a7 126 }
s1588141 5:4b2ff2a4664a 127
s1588141 5:4b2ff2a4664a 128
s1588141 4:8b1df22779a7 129
s1588141 5:4b2ff2a4664a 130 //-------------------------------PI ---------------------------------
s1588141 4:8b1df22779a7 131
s1588141 5:4b2ff2a4664a 132 double PI(double e, const double Kp, const double Ki, double Sf,
s1588141 5:4b2ff2a4664a 133 double &e_int){
s1588141 4:8b1df22779a7 134 //This function is a PID controller that returns the errorsignal for the plant
s1588141 4:8b1df22779a7 135
s1588141 4:8b1df22779a7 136
s1588141 4:8b1df22779a7 137 e_int = e_int + Sf * e;
s1588141 4:8b1df22779a7 138 // PID
s1588141 5:4b2ff2a4664a 139 return Kp*e + Ki*e_int ;
s1588141 1:d8bb72c9c019 140 }
s1588141 4:8b1df22779a7 141
s1588141 4:8b1df22779a7 142 //---------------------------Controller---------------------------------------
s1588141 4:8b1df22779a7 143
s1588141 4:8b1df22779a7 144
s1588141 4:8b1df22779a7 145
s1588141 4:8b1df22779a7 146 // Controller gains (motor1−Kp,−Ki,...)
s1588141 5:4b2ff2a4664a 147 const double Kpm= 0.1, Kim = 0.05;
s1588141 5:4b2ff2a4664a 148 double er_int = 0;
s1588141 5:4b2ff2a4664a 149
s1588141 4:8b1df22779a7 150 double Controller_L(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 151 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 152 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 153
s1588141 4:8b1df22779a7 154 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 155 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 156
s1588141 4:8b1df22779a7 157 //PID controller
s1588141 5:4b2ff2a4664a 158 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int );
s1588141 4:8b1df22779a7 159 //Determining rotational direction of the motor
s1588141 4:8b1df22779a7 160 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 161 M_L_D = 1;
s1588141 4:8b1df22779a7 162 } else {
s1588141 4:8b1df22779a7 163 M_L_D = 0;
s1588141 4:8b1df22779a7 164 }
s1588141 4:8b1df22779a7 165 return fabs(Speed_Set);
s1588141 4:8b1df22779a7 166 }
s1588141 4:8b1df22779a7 167
s1588141 4:8b1df22779a7 168 double Controller_R(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 169 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 170 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 171
s1588141 4:8b1df22779a7 172 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 173 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 174
s1588141 5:4b2ff2a4664a 175 //PI controller
s1588141 5:4b2ff2a4664a 176 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int);
s1588141 4:8b1df22779a7 177 //Determining rotational direction of the motor
s1588141 4:8b1df22779a7 178 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 179 M_R_D = 0;
s1588141 4:8b1df22779a7 180 } else {
s1588141 4:8b1df22779a7 181 M_R_D = 1;
s1588141 4:8b1df22779a7 182 }
s1588141 4:8b1df22779a7 183 if (fabs(Speed_Set)< 0.4){
s1588141 5:4b2ff2a4664a 184 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
s1588141 5:4b2ff2a4664a 185 return 0;
s1588141 5:4b2ff2a4664a 186 } else {
s1588141 4:8b1df22779a7 187 return fabs(Speed_Set);
s1588141 5:4b2ff2a4664a 188 }
s1588141 4:8b1df22779a7 189 } else {
s1588141 4:8b1df22779a7 190 return 0.4;
s1588141 4:8b1df22779a7 191
s1588141 4:8b1df22779a7 192 }
s1588141 4:8b1df22779a7 193 }
s1588141 4:8b1df22779a7 194 //-----------------------------Change_Movement----------------------------------
s1588141 4:8b1df22779a7 195 // This function changes the movement type of the robot as a consequence of the input by EMG
s1588141 4:8b1df22779a7 196 // Or when the EMG signal is hold for 1.5S The grapler wil activate
s1588141 4:8b1df22779a7 197 Ticker Tick_Movement;
s1588141 4:8b1df22779a7 198 int check = 0;
s1588141 4:8b1df22779a7 199
s1588141 4:8b1df22779a7 200 bool Movement_Flag = false;
s1588141 4:8b1df22779a7 201 void Set_movement_Flag(){
s1588141 4:8b1df22779a7 202 Movement_Flag = true;
s1588141 4:8b1df22779a7 203 }
s1588141 4:8b1df22779a7 204
s1588141 4:8b1df22779a7 205 void Change_Movement(){
s1588141 4:8b1df22779a7 206 if (Movement_Flag == true){
s1588141 4:8b1df22779a7 207 if (EMG_Change > 0.5f ){
s1588141 4:8b1df22779a7 208 check += 1;
s1588141 4:8b1df22779a7 209 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 210 grip1 = 1;
s1588141 4:8b1df22779a7 211 }
s1588141 4:8b1df22779a7 212 if (check > 6){
s1588141 4:8b1df22779a7 213 grip2 =1;
s1588141 4:8b1df22779a7 214 }
s1588141 4:8b1df22779a7 215 } else {
s1588141 4:8b1df22779a7 216 //Hold the signal shorter than 1.5 seconds
s1588141 4:8b1df22779a7 217 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 218 movement = !movement;
s1588141 4:8b1df22779a7 219 grip1 = 1;
s1588141 4:8b1df22779a7 220 }
s1588141 4:8b1df22779a7 221 // Hold the signal longer than 1.5 seconds
s1588141 4:8b1df22779a7 222 if (check > 6){
s1588141 4:8b1df22779a7 223 grip2 =1;
s1588141 4:8b1df22779a7 224 Grap = !Grap;
s1588141 4:8b1df22779a7 225
s1588141 4:8b1df22779a7 226 }
s1588141 4:8b1df22779a7 227
s1588141 4:8b1df22779a7 228 grip2 = 0;
s1588141 4:8b1df22779a7 229 grip1 =0;
s1588141 4:8b1df22779a7 230 check = 0;
s1588141 4:8b1df22779a7 231
s1588141 4:8b1df22779a7 232 }
s1588141 4:8b1df22779a7 233 Movement_Flag = false;
s1588141 4:8b1df22779a7 234 }
s1588141 2:0954eb04bb4c 235 }
s1588141 5:4b2ff2a4664a 236 //-----------------------------EMG Signal determinator----------------------
s1588141 5:4b2ff2a4664a 237 BiQuadChain bqc_L;
s1588141 5:4b2ff2a4664a 238 BiQuadChain bqc_R;
s1588141 6:aa62e6650559 239 BiQuadChain bqc_Change;
s1588141 5:4b2ff2a4664a 240 BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 241 BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 242 BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 243
s1588141 5:4b2ff2a4664a 244 BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 245 BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 246 BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 247
s1588141 6:aa62e6650559 248
s1588141 6:aa62e6650559 249 BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 6:aa62e6650559 250 BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 6:aa62e6650559 251 BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 6:aa62e6650559 252
s1588141 6:aa62e6650559 253 void getEMG(){
s1588141 6:aa62e6650559 254
s1588141 5:4b2ff2a4664a 255
s1588141 6:aa62e6650559 256 EMG_Change = bqc_Change.step(Change.read());
s1588141 6:aa62e6650559 257 EMG_Change = fabs(EMG_Change);
s1588141 6:aa62e6650559 258 EMG_Change = bq3_Change.step(EMG_Change)*1.0;
s1588141 6:aa62e6650559 259 scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope
s1588141 6:aa62e6650559 260 if (EMG_Change < 0.3){
s1588141 6:aa62e6650559 261 EMG_Change=0;
s1588141 6:aa62e6650559 262 } else {
s1588141 6:aa62e6650559 263 EMG_Change=1.0;
s1588141 6:aa62e6650559 264 }
s1588141 6:aa62e6650559 265
s1588141 6:aa62e6650559 266
s1588141 6:aa62e6650559 267 EMG_L = bqc_L.step(Left.read());
s1588141 6:aa62e6650559 268 EMG_L= fabs(EMG_L);
s1588141 6:aa62e6650559 269 EMG_L= bq3_L.step( EMG_L)*1.0;
s1588141 6:aa62e6650559 270 scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope
s1588141 6:aa62e6650559 271 if (EMG_L < 0.3){
s1588141 6:aa62e6650559 272 EMG_L=0;
s1588141 6:aa62e6650559 273 } else {
s1588141 6:aa62e6650559 274 EMG_L=1.0;
s1588141 6:aa62e6650559 275 }
s1588141 6:aa62e6650559 276
s1588141 6:aa62e6650559 277
s1588141 5:4b2ff2a4664a 278
s1588141 6:aa62e6650559 279 EMG_R = bqc_R.step(Right.read());
s1588141 6:aa62e6650559 280 EMG_R= fabs(EMG_R);
s1588141 6:aa62e6650559 281 EMG_R= bq3_R.step( EMG_R)*1.0;
s1588141 6:aa62e6650559 282 scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope
s1588141 6:aa62e6650559 283 if (EMG_R < 0.3){
s1588141 6:aa62e6650559 284 EMG_R=0;
s1588141 6:aa62e6650559 285 } else {
s1588141 6:aa62e6650559 286 EMG_R= 1.0;
s1588141 6:aa62e6650559 287 }
s1588141 5:4b2ff2a4664a 288
s1588141 5:4b2ff2a4664a 289 }
s1588141 6:aa62e6650559 290
s1588141 6:aa62e6650559 291 /////////////-----------------------Booting---------------------------------------=
s1588141 6:aa62e6650559 292 void Boot(){
s1588141 6:aa62e6650559 293
s1588141 6:aa62e6650559 294 //Fucnction that initializes variables that have to be initalized within the main of the script
s1588141 6:aa62e6650559 295 //And does the same thing for functions like encoder reset
s1588141 6:aa62e6650559 296 pc.baud(115200);
s1588141 6:aa62e6650559 297 pc.printf("\r\n**BOARD RESET**\r\n");
s1588141 6:aa62e6650559 298
s1588141 6:aa62e6650559 299 M_L_S.period(1.0/Motor_Frequency);
s1588141 6:aa62e6650559 300 M_L_S = 0.0;
s1588141 6:aa62e6650559 301 M_R_S.period(1.0/Motor_Frequency);
s1588141 6:aa62e6650559 302 M_R_S= 0.0;
s1588141 6:aa62e6650559 303 rotation = 1;
s1588141 6:aa62e6650559 304 translation = 0;
s1588141 6:aa62e6650559 305 Encoder_L.reset();
s1588141 6:aa62e6650559 306 Encoder_R.reset();
s1588141 6:aa62e6650559 307
s1588141 6:aa62e6650559 308 Grap = 1;
s1588141 6:aa62e6650559 309 grip1 = 1;
s1588141 6:aa62e6650559 310
s1588141 6:aa62e6650559 311 bqc_L.add( &bq1_L ).add( &bq2_L );
s1588141 6:aa62e6650559 312 bqc_R.add(&bq1_R).add(&bq2_R);
s1588141 6:aa62e6650559 313 bqc_Change.add(&bq1_Change).add(&bq2_Change);
s1588141 6:aa62e6650559 314 Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency); Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
s1588141 6:aa62e6650559 315 Tick_Movement.attach(Set_movement_Flag, 0.25);
s1588141 6:aa62e6650559 316 OnOff.fall(Start_Stop);
s1588141 6:aa62e6650559 317 }
s1588141 6:aa62e6650559 318
s1588141 6:aa62e6650559 319
s1588141 6:aa62e6650559 320
s1588141 0:a2db8cc5d5df 321 int main()
s1588141 0:a2db8cc5d5df 322 {
s1588141 4:8b1df22779a7 323 //---------------------Setting constants and system booting---------------------
s1588141 4:8b1df22779a7 324
s1588141 0:a2db8cc5d5df 325 Boot();
s1588141 6:aa62e6650559 326
s1588141 4:8b1df22779a7 327 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 328 //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 329 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 330 while (true) {
s1588141 6:aa62e6650559 331 if (Sample_Flag == true){
s1588141 4:8b1df22779a7 332 Sample();
s1588141 6:aa62e6650559 333 getEMG();
s1588141 6:aa62e6650559 334 // EMG_Change = getEMG( bqc.step(Change.read()));
s1588141 5:4b2ff2a4664a 335 Signal = pi*(EMG_R-EMG_L);
s1588141 6:aa62e6650559 336 scope.set(3,Signal);
s1588141 5:4b2ff2a4664a 337
s1588141 5:4b2ff2a4664a 338 scope.send();
s1588141 4:8b1df22779a7 339 Sample_Flag = false;
s1588141 4:8b1df22779a7 340 }
s1588141 4:8b1df22779a7 341 //Main statement that states if the system is active or not
s1588141 4:8b1df22779a7 342 if (Active == true){
s1588141 2:0954eb04bb4c 343 green = 0;
s1588141 4:8b1df22779a7 344 red = 1;
s1588141 4:8b1df22779a7 345 Change_Movement();
s1588141 4:8b1df22779a7 346 if (Controller_Flag == true){
s1588141 4:8b1df22779a7 347 switch (movement) {
s1588141 4:8b1df22779a7 348 case 0:
s1588141 4:8b1df22779a7 349 //Clockwise rotation of the robot
s1588141 4:8b1df22779a7 350 direction_L = -1;
s1588141 4:8b1df22779a7 351 direction_R = -1;
s1588141 6:aa62e6650559 352 rotation = 1;
s1588141 6:aa62e6650559 353 translation = 0;
s1588141 4:8b1df22779a7 354 break;
s1588141 4:8b1df22779a7 355
s1588141 4:8b1df22779a7 356 case 1:
s1588141 4:8b1df22779a7 357 //Radial movement outwards for a positive value
s1588141 4:8b1df22779a7 358 direction_L = 1;
s1588141 4:8b1df22779a7 359 direction_R = -1;
s1588141 6:aa62e6650559 360 rotation = 0;
s1588141 6:aa62e6650559 361 translation = 1;
s1588141 4:8b1df22779a7 362 break;
s1588141 4:8b1df22779a7 363 }
s1588141 4:8b1df22779a7 364 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 4:8b1df22779a7 365 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 4:8b1df22779a7 366 Controller_Flag = false;
s1588141 4:8b1df22779a7 367 }
s1588141 1:d8bb72c9c019 368 } else {
s1588141 4:8b1df22779a7 369 if (Active == false){
s1588141 4:8b1df22779a7 370 green = 1;
s1588141 4:8b1df22779a7 371 red = 0;
s1588141 4:8b1df22779a7 372 M_R_S = 0;
s1588141 1:d8bb72c9c019 373 M_L_S = 0;
s1588141 4:8b1df22779a7 374 }
s1588141 2:0954eb04bb4c 375 }
s1588141 0:a2db8cc5d5df 376 }
s1588141 4:8b1df22779a7 377 }
s1588141 4:8b1df22779a7 378