Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

main.cpp@4:8b1df22779a7, 2016-10-25 (annotated)

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