Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

main.cpp@5:4b2ff2a4664a, 2016-10-28 (annotated)

Committer:
s1588141
Date:
Fri Oct 28 10:26:11 2016 +0000
Revision:
5:4b2ff2a4664a
Parent:
4:8b1df22779a7
Child:
6:aa62e6650559
EMG Working;

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 /////////////-----------------------Booting---------------------------------------=
s1588141 0:a2db8cc5d5df 90 void Boot(){
s1588141 0:a2db8cc5d5df 91
s1588141 4:8b1df22779a7 92 //Fucnction that initializes variables that have to be initalized within the main of the script
s1588141 4:8b1df22779a7 93 //And does the same thing for functions like encoder reset
s1588141 4:8b1df22779a7 94 pc.baud(115200);
s1588141 4:8b1df22779a7 95 pc.printf("\r\n**BOARD RESET**\r\n");
s1588141 4:8b1df22779a7 96
s1588141 0:a2db8cc5d5df 97 M_L_S.period(1.0/Motor_Frequency);
s1588141 0:a2db8cc5d5df 98 M_L_S = 0.0;
s1588141 0:a2db8cc5d5df 99 M_R_S.period(1.0/Motor_Frequency);
s1588141 0:a2db8cc5d5df 100 M_R_S= 0.0;
s1588141 2:0954eb04bb4c 101
s1588141 2:0954eb04bb4c 102 Encoder_L.reset();
s1588141 2:0954eb04bb4c 103 Encoder_R.reset();
s1588141 4:8b1df22779a7 104
s1588141 4:8b1df22779a7 105 Grap = 1;
s1588141 4:8b1df22779a7 106 grip1 = 1;
s1588141 0:a2db8cc5d5df 107 }
s1588141 4:8b1df22779a7 108
s1588141 4:8b1df22779a7 109
s1588141 4:8b1df22779a7 110
s1588141 4:8b1df22779a7 111 //--------------------------------Sampling------------------------------------------
s1588141 5:4b2ff2a4664a 112
s1588141 5:4b2ff2a4664a 113 Ticker Tick_Sample;// ticker for data sampling
s1588141 5:4b2ff2a4664a 114
s1588141 4:8b1df22779a7 115 bool Sample_Flag = false;
s1588141 4:8b1df22779a7 116
s1588141 4:8b1df22779a7 117 void Set_Sample_Flag(){
s1588141 4:8b1df22779a7 118 Sample_Flag = true;
s1588141 0:a2db8cc5d5df 119 }
s1588141 0:a2db8cc5d5df 120
s1588141 4:8b1df22779a7 121 void Sample(){
s1588141 1:d8bb72c9c019 122
s1588141 4:8b1df22779a7 123 //This function reads out the position and the speed of the motors in radiants
s1588141 4:8b1df22779a7 124 //Aand stores them in the values Speed and Position
s1588141 4:8b1df22779a7 125
s1588141 2:0954eb04bb4c 126 //Position in Radials:
s1588141 2:0954eb04bb4c 127 Previous_Position_L = Position_L;
s1588141 2:0954eb04bb4c 128 Previous_Position_R = Position_R;
s1588141 2:0954eb04bb4c 129 Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
s1588141 2:0954eb04bb4c 130 Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
s1588141 2:0954eb04bb4c 131
s1588141 2:0954eb04bb4c 132 //Speed in RadPers second
s1588141 4:8b1df22779a7 133 Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
s1588141 4:8b1df22779a7 134 Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
s1588141 4:8b1df22779a7 135
s1588141 4:8b1df22779a7 136 Encoder_L.reset();
s1588141 4:8b1df22779a7 137 Encoder_R.reset();
s1588141 4:8b1df22779a7 138
s1588141 4:8b1df22779a7 139 }
s1588141 4:8b1df22779a7 140
s1588141 4:8b1df22779a7 141 bool Controller_Flag = false;
s1588141 4:8b1df22779a7 142
s1588141 4:8b1df22779a7 143 Ticker Tick_Controller;
s1588141 4:8b1df22779a7 144
s1588141 4:8b1df22779a7 145 void Set_controller_Flag(){
s1588141 4:8b1df22779a7 146 Controller_Flag = true;
s1588141 4:8b1df22779a7 147 }
s1588141 5:4b2ff2a4664a 148
s1588141 5:4b2ff2a4664a 149
s1588141 4:8b1df22779a7 150
s1588141 5:4b2ff2a4664a 151 //-------------------------------PI ---------------------------------
s1588141 4:8b1df22779a7 152
s1588141 5:4b2ff2a4664a 153 double PI(double e, const double Kp, const double Ki, double Sf,
s1588141 5:4b2ff2a4664a 154 double &e_int){
s1588141 4:8b1df22779a7 155 //This function is a PID controller that returns the errorsignal for the plant
s1588141 4:8b1df22779a7 156
s1588141 4:8b1df22779a7 157
s1588141 4:8b1df22779a7 158 e_int = e_int + Sf * e;
s1588141 4:8b1df22779a7 159 // PID
s1588141 5:4b2ff2a4664a 160 return Kp*e + Ki*e_int ;
s1588141 1:d8bb72c9c019 161 }
s1588141 4:8b1df22779a7 162
s1588141 4:8b1df22779a7 163 //---------------------------Controller---------------------------------------
s1588141 4:8b1df22779a7 164
s1588141 4:8b1df22779a7 165
s1588141 4:8b1df22779a7 166
s1588141 4:8b1df22779a7 167 // Controller gains (motor1−Kp,−Ki,...)
s1588141 5:4b2ff2a4664a 168 const double Kpm= 0.1, Kim = 0.05;
s1588141 5:4b2ff2a4664a 169 double er_int = 0;
s1588141 5:4b2ff2a4664a 170
s1588141 4:8b1df22779a7 171 double Controller_L(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 172 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 173 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 174
s1588141 4:8b1df22779a7 175 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 176 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 177
s1588141 4:8b1df22779a7 178 //PID controller
s1588141 5:4b2ff2a4664a 179 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int );
s1588141 4:8b1df22779a7 180 //Determining rotational direction of the motor
s1588141 4:8b1df22779a7 181 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 182 M_L_D = 1;
s1588141 4:8b1df22779a7 183 } else {
s1588141 4:8b1df22779a7 184 M_L_D = 0;
s1588141 4:8b1df22779a7 185 }
s1588141 4:8b1df22779a7 186 return fabs(Speed_Set);
s1588141 4:8b1df22779a7 187 }
s1588141 4:8b1df22779a7 188
s1588141 4:8b1df22779a7 189 double Controller_R(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 190 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 191 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 192
s1588141 4:8b1df22779a7 193 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 194 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 195
s1588141 5:4b2ff2a4664a 196 //PI controller
s1588141 5:4b2ff2a4664a 197 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, er_int);
s1588141 4:8b1df22779a7 198 //Determining rotational direction of the motor
s1588141 4:8b1df22779a7 199 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 200 M_R_D = 0;
s1588141 4:8b1df22779a7 201 } else {
s1588141 4:8b1df22779a7 202 M_R_D = 1;
s1588141 4:8b1df22779a7 203 }
s1588141 4:8b1df22779a7 204 if (fabs(Speed_Set)< 0.4){
s1588141 5:4b2ff2a4664a 205 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
s1588141 5:4b2ff2a4664a 206 return 0;
s1588141 5:4b2ff2a4664a 207 } else {
s1588141 4:8b1df22779a7 208 return fabs(Speed_Set);
s1588141 5:4b2ff2a4664a 209 }
s1588141 4:8b1df22779a7 210 } else {
s1588141 4:8b1df22779a7 211 return 0.4;
s1588141 4:8b1df22779a7 212
s1588141 4:8b1df22779a7 213 }
s1588141 4:8b1df22779a7 214 }
s1588141 4:8b1df22779a7 215 //-----------------------------Change_Movement----------------------------------
s1588141 4:8b1df22779a7 216 // This function changes the movement type of the robot as a consequence of the input by EMG
s1588141 4:8b1df22779a7 217 // Or when the EMG signal is hold for 1.5S The grapler wil activate
s1588141 4:8b1df22779a7 218 Ticker Tick_Movement;
s1588141 4:8b1df22779a7 219 int check = 0;
s1588141 4:8b1df22779a7 220
s1588141 4:8b1df22779a7 221 bool Movement_Flag = false;
s1588141 4:8b1df22779a7 222 void Set_movement_Flag(){
s1588141 4:8b1df22779a7 223 Movement_Flag = true;
s1588141 4:8b1df22779a7 224 }
s1588141 4:8b1df22779a7 225
s1588141 4:8b1df22779a7 226 void Change_Movement(){
s1588141 4:8b1df22779a7 227 if (Movement_Flag == true){
s1588141 4:8b1df22779a7 228 if (EMG_Change > 0.5f ){
s1588141 4:8b1df22779a7 229 check += 1;
s1588141 4:8b1df22779a7 230 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 231 grip1 = 1;
s1588141 4:8b1df22779a7 232 }
s1588141 4:8b1df22779a7 233 if (check > 6){
s1588141 4:8b1df22779a7 234 grip2 =1;
s1588141 4:8b1df22779a7 235 }
s1588141 4:8b1df22779a7 236 } else {
s1588141 4:8b1df22779a7 237 //Hold the signal shorter than 1.5 seconds
s1588141 4:8b1df22779a7 238 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 239 movement = !movement;
s1588141 4:8b1df22779a7 240 grip1 = 1;
s1588141 4:8b1df22779a7 241 }
s1588141 4:8b1df22779a7 242 // Hold the signal longer than 1.5 seconds
s1588141 4:8b1df22779a7 243 if (check > 6){
s1588141 4:8b1df22779a7 244 grip2 =1;
s1588141 4:8b1df22779a7 245 Grap = !Grap;
s1588141 4:8b1df22779a7 246
s1588141 4:8b1df22779a7 247 }
s1588141 4:8b1df22779a7 248
s1588141 4:8b1df22779a7 249 grip2 = 0;
s1588141 4:8b1df22779a7 250 grip1 =0;
s1588141 4:8b1df22779a7 251 check = 0;
s1588141 4:8b1df22779a7 252
s1588141 4:8b1df22779a7 253 }
s1588141 4:8b1df22779a7 254 Movement_Flag = false;
s1588141 4:8b1df22779a7 255 }
s1588141 2:0954eb04bb4c 256 }
s1588141 5:4b2ff2a4664a 257 //-----------------------------EMG Signal determinator----------------------
s1588141 5:4b2ff2a4664a 258 BiQuadChain bqc_L;
s1588141 5:4b2ff2a4664a 259 BiQuadChain bqc_R;
s1588141 5:4b2ff2a4664a 260 BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 261 BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 262 BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 263
s1588141 5:4b2ff2a4664a 264 BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 265 BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 266 BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 267
s1588141 5:4b2ff2a4664a 268 double getEMG(double EMG_Out){
s1588141 5:4b2ff2a4664a 269
s1588141 5:4b2ff2a4664a 270
s1588141 5:4b2ff2a4664a 271
s1588141 5:4b2ff2a4664a 272 return EMG_Out;
s1588141 5:4b2ff2a4664a 273 }
s1588141 0:a2db8cc5d5df 274 int main()
s1588141 0:a2db8cc5d5df 275 {
s1588141 4:8b1df22779a7 276 //---------------------Setting constants and system booting---------------------
s1588141 4:8b1df22779a7 277
s1588141 0:a2db8cc5d5df 278 Boot();
s1588141 5:4b2ff2a4664a 279 bqc_L.add( &bq1_L ).add( &bq2_L );
s1588141 5:4b2ff2a4664a 280 bqc_R.add(&bq1_R).add(&bq2_R);
s1588141 4:8b1df22779a7 281 Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency); Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
s1588141 4:8b1df22779a7 282 Tick_Movement.attach(Set_movement_Flag, 0.25);
s1588141 4:8b1df22779a7 283 OnOff.fall(Start_Stop);
s1588141 4:8b1df22779a7 284 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 285 //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 286 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 287 while (true) {
s1588141 4:8b1df22779a7 288 if (Sample_Flag == true){
s1588141 4:8b1df22779a7 289 Sample();
s1588141 5:4b2ff2a4664a 290 EMG_L = bqc_L.step(Left.read());
s1588141 5:4b2ff2a4664a 291 EMG_L= fabs(EMG_L);
s1588141 5:4b2ff2a4664a 292 EMG_L= bq3_L.step( EMG_L)*10.0;
s1588141 5:4b2ff2a4664a 293 scope.set(0,EMG_L);
s1588141 5:4b2ff2a4664a 294 if (EMG_L < 0.2){
s1588141 5:4b2ff2a4664a 295 EMG_L=0;
s1588141 5:4b2ff2a4664a 296 } else if (EMG_L >= 0.2 && EMG_L < 0.5){
s1588141 5:4b2ff2a4664a 297 EMG_L=0.33;
s1588141 5:4b2ff2a4664a 298 } else if (EMG_L >= 0.5 && EMG_L < 0.8){
s1588141 5:4b2ff2a4664a 299 EMG_L=0.67;
s1588141 5:4b2ff2a4664a 300 } else {
s1588141 5:4b2ff2a4664a 301 EMG_L=1.0;
s1588141 5:4b2ff2a4664a 302 }
s1588141 5:4b2ff2a4664a 303
s1588141 5:4b2ff2a4664a 304 scope.set(1,EMG_L);
s1588141 5:4b2ff2a4664a 305
s1588141 5:4b2ff2a4664a 306 EMG_R = bqc_R.step(Right.read());
s1588141 5:4b2ff2a4664a 307 EMG_R= fabs(EMG_R);
s1588141 5:4b2ff2a4664a 308 EMG_R= bq3_R.step( EMG_R)*10.0;
s1588141 5:4b2ff2a4664a 309
s1588141 5:4b2ff2a4664a 310 scope.set(2,EMG_R);
s1588141 5:4b2ff2a4664a 311 if (EMG_R < 0.2){
s1588141 5:4b2ff2a4664a 312 EMG_R=0;
s1588141 5:4b2ff2a4664a 313 } else if (EMG_R >= 0.2 && EMG_R < 0.5){
s1588141 5:4b2ff2a4664a 314 EMG_R=0.33;
s1588141 5:4b2ff2a4664a 315 } else if (EMG_R >= 0.5 && EMG_R < 0.8){
s1588141 5:4b2ff2a4664a 316 EMG_R=0.67;
s1588141 5:4b2ff2a4664a 317 } else {
s1588141 5:4b2ff2a4664a 318 EMG_R= 1.0;
s1588141 5:4b2ff2a4664a 319 }
s1588141 5:4b2ff2a4664a 320
s1588141 5:4b2ff2a4664a 321 scope.set(3,EMG_R);
s1588141 5:4b2ff2a4664a 322
s1588141 5:4b2ff2a4664a 323 // EMG_Change = getEMG( bqc.step(Change.read()));
s1588141 5:4b2ff2a4664a 324 EMG_Change = 0;
s1588141 5:4b2ff2a4664a 325 Signal = pi*(EMG_R-EMG_L);
s1588141 5:4b2ff2a4664a 326
s1588141 5:4b2ff2a4664a 327
s1588141 5:4b2ff2a4664a 328 scope.send();
s1588141 4:8b1df22779a7 329 Sample_Flag = false;
s1588141 4:8b1df22779a7 330 }
s1588141 4:8b1df22779a7 331 //Main statement that states if the system is active or not
s1588141 4:8b1df22779a7 332 if (Active == true){
s1588141 2:0954eb04bb4c 333 green = 0;
s1588141 4:8b1df22779a7 334 red = 1;
s1588141 4:8b1df22779a7 335 Change_Movement();
s1588141 4:8b1df22779a7 336 if (Controller_Flag == true){
s1588141 4:8b1df22779a7 337 switch (movement) {
s1588141 4:8b1df22779a7 338 case 0:
s1588141 4:8b1df22779a7 339 //Clockwise rotation of the robot
s1588141 4:8b1df22779a7 340 direction_L = -1;
s1588141 4:8b1df22779a7 341 direction_R = -1;
s1588141 4:8b1df22779a7 342 break;
s1588141 4:8b1df22779a7 343
s1588141 4:8b1df22779a7 344 case 1:
s1588141 4:8b1df22779a7 345 //Radial movement outwards for a positive value
s1588141 4:8b1df22779a7 346 direction_L = 1;
s1588141 4:8b1df22779a7 347 direction_R = -1;
s1588141 4:8b1df22779a7 348 break;
s1588141 4:8b1df22779a7 349 }
s1588141 4:8b1df22779a7 350 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 4:8b1df22779a7 351 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 4:8b1df22779a7 352 Controller_Flag = false;
s1588141 4:8b1df22779a7 353 }
s1588141 1:d8bb72c9c019 354 } else {
s1588141 4:8b1df22779a7 355 if (Active == false){
s1588141 4:8b1df22779a7 356 green = 1;
s1588141 4:8b1df22779a7 357 red = 0;
s1588141 4:8b1df22779a7 358 M_R_S = 0;
s1588141 1:d8bb72c9c019 359 M_L_S = 0;
s1588141 4:8b1df22779a7 360 }
s1588141 2:0954eb04bb4c 361 }
s1588141 0:a2db8cc5d5df 362 }
s1588141 4:8b1df22779a7 363 }
s1588141 4:8b1df22779a7 364