Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

main.cpp@8:656b0c493a45, 2016-11-01 (annotated)

Committer:
s1588141
Date:
Tue Nov 01 11:18:41 2016 +0000
Revision:
8:656b0c493a45
Parent:
7:075ba23f3147
Child:
9:e764cb50d343
Limits callibrated;

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 7:075ba23f3147 34 const int SampleFrequency = 400;
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 7:075ba23f3147 55
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 8:656b0c493a45 147 const double Kpm= 0.1, Kim = 0.005;
s1588141 8:656b0c493a45 148 double e_L_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 8:656b0c493a45 158 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_L_int );
s1588141 4:8b1df22779a7 159 //Determining rotational direction of the motor
s1588141 8:656b0c493a45 160 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 161 M_L_D = 1;
s1588141 4:8b1df22779a7 162 } else {
s1588141 8:656b0c493a45 163 M_L_D = 0;
s1588141 4:8b1df22779a7 164 }
s1588141 8:656b0c493a45 165 if (fabs(Speed_Set)< 0.4){
s1588141 8:656b0c493a45 166 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
s1588141 8:656b0c493a45 167 return 0;
s1588141 8:656b0c493a45 168 } else {
s1588141 4:8b1df22779a7 169 return fabs(Speed_Set);
s1588141 8:656b0c493a45 170 }
s1588141 8:656b0c493a45 171 } else {
s1588141 8:656b0c493a45 172 return 0.4;
s1588141 8:656b0c493a45 173
s1588141 8:656b0c493a45 174 }
s1588141 8:656b0c493a45 175 }
s1588141 8:656b0c493a45 176 double e_R_int = 0;
s1588141 4:8b1df22779a7 177 double Controller_R(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 178 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 179 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 180
s1588141 4:8b1df22779a7 181 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 182 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 183
s1588141 5:4b2ff2a4664a 184 //PI controller
s1588141 8:656b0c493a45 185 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_R_int);
s1588141 4:8b1df22779a7 186 //Determining rotational direction of the motor
s1588141 4:8b1df22779a7 187 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 188 M_R_D = 0;
s1588141 4:8b1df22779a7 189 } else {
s1588141 4:8b1df22779a7 190 M_R_D = 1;
s1588141 4:8b1df22779a7 191 }
s1588141 4:8b1df22779a7 192 if (fabs(Speed_Set)< 0.4){
s1588141 5:4b2ff2a4664a 193 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
s1588141 5:4b2ff2a4664a 194 return 0;
s1588141 5:4b2ff2a4664a 195 } else {
s1588141 4:8b1df22779a7 196 return fabs(Speed_Set);
s1588141 5:4b2ff2a4664a 197 }
s1588141 4:8b1df22779a7 198 } else {
s1588141 4:8b1df22779a7 199 return 0.4;
s1588141 4:8b1df22779a7 200
s1588141 4:8b1df22779a7 201 }
s1588141 4:8b1df22779a7 202 }
s1588141 4:8b1df22779a7 203 //-----------------------------Change_Movement----------------------------------
s1588141 4:8b1df22779a7 204 // This function changes the movement type of the robot as a consequence of the input by EMG
s1588141 4:8b1df22779a7 205 // Or when the EMG signal is hold for 1.5S The grapler wil activate
s1588141 4:8b1df22779a7 206 Ticker Tick_Movement;
s1588141 4:8b1df22779a7 207 int check = 0;
s1588141 4:8b1df22779a7 208
s1588141 4:8b1df22779a7 209 bool Movement_Flag = false;
s1588141 4:8b1df22779a7 210 void Set_movement_Flag(){
s1588141 4:8b1df22779a7 211 Movement_Flag = true;
s1588141 4:8b1df22779a7 212 }
s1588141 4:8b1df22779a7 213
s1588141 4:8b1df22779a7 214 void Change_Movement(){
s1588141 4:8b1df22779a7 215 if (Movement_Flag == true){
s1588141 4:8b1df22779a7 216 if (EMG_Change > 0.5f ){
s1588141 4:8b1df22779a7 217 check += 1;
s1588141 7:075ba23f3147 218 if (check <= 10 and check > 1 ){
s1588141 4:8b1df22779a7 219 grip1 = 1;
s1588141 4:8b1df22779a7 220 }
s1588141 7:075ba23f3147 221 if (check > 10){
s1588141 4:8b1df22779a7 222 grip2 =1;
s1588141 4:8b1df22779a7 223 }
s1588141 4:8b1df22779a7 224 } else {
s1588141 4:8b1df22779a7 225 //Hold the signal shorter than 1.5 seconds
s1588141 4:8b1df22779a7 226 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 227 movement = !movement;
s1588141 4:8b1df22779a7 228 grip1 = 1;
s1588141 4:8b1df22779a7 229 }
s1588141 4:8b1df22779a7 230 // Hold the signal longer than 1.5 seconds
s1588141 4:8b1df22779a7 231 if (check > 6){
s1588141 4:8b1df22779a7 232 grip2 =1;
s1588141 4:8b1df22779a7 233 Grap = !Grap;
s1588141 4:8b1df22779a7 234
s1588141 4:8b1df22779a7 235 }
s1588141 4:8b1df22779a7 236
s1588141 4:8b1df22779a7 237 grip2 = 0;
s1588141 4:8b1df22779a7 238 grip1 =0;
s1588141 4:8b1df22779a7 239 check = 0;
s1588141 4:8b1df22779a7 240
s1588141 4:8b1df22779a7 241 }
s1588141 4:8b1df22779a7 242 Movement_Flag = false;
s1588141 4:8b1df22779a7 243 }
s1588141 2:0954eb04bb4c 244 }
s1588141 5:4b2ff2a4664a 245 //-----------------------------EMG Signal determinator----------------------
s1588141 5:4b2ff2a4664a 246 BiQuadChain bqc_L;
s1588141 5:4b2ff2a4664a 247 BiQuadChain bqc_R;
s1588141 6:aa62e6650559 248 BiQuadChain bqc_Change;
s1588141 5:4b2ff2a4664a 249 BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 250 BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 251 BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 252
s1588141 5:4b2ff2a4664a 253 BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 254 BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 255 BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 256
s1588141 6:aa62e6650559 257
s1588141 6:aa62e6650559 258 BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 6:aa62e6650559 259 BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 6:aa62e6650559 260 BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 7:075ba23f3147 261 HIDScope scope(5); // Sending two sets of data
s1588141 6:aa62e6650559 262 void getEMG(){
s1588141 6:aa62e6650559 263
s1588141 5:4b2ff2a4664a 264
s1588141 6:aa62e6650559 265 EMG_Change = bqc_Change.step(Change.read());
s1588141 6:aa62e6650559 266 EMG_Change = fabs(EMG_Change);
s1588141 7:075ba23f3147 267 EMG_Change = bq3_Change.step(EMG_Change)*50;
s1588141 7:075ba23f3147 268 scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope
s1588141 7:075ba23f3147 269 scope.set(4,Change.read());
s1588141 6:aa62e6650559 270 if (EMG_Change < 0.3){
s1588141 6:aa62e6650559 271 EMG_Change=0;
s1588141 6:aa62e6650559 272 } else {
s1588141 6:aa62e6650559 273 EMG_Change=1.0;
s1588141 6:aa62e6650559 274 }
s1588141 6:aa62e6650559 275
s1588141 6:aa62e6650559 276
s1588141 6:aa62e6650559 277 EMG_L = bqc_L.step(Left.read());
s1588141 6:aa62e6650559 278 EMG_L= fabs(EMG_L);
s1588141 7:075ba23f3147 279 EMG_L= bq3_L.step( EMG_L)*10;
s1588141 7:075ba23f3147 280 scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope
s1588141 7:075ba23f3147 281
s1588141 6:aa62e6650559 282 if (EMG_L < 0.3){
s1588141 6:aa62e6650559 283 EMG_L=0;
s1588141 6:aa62e6650559 284 } else {
s1588141 6:aa62e6650559 285 EMG_L=1.0;
s1588141 6:aa62e6650559 286 }
s1588141 6:aa62e6650559 287
s1588141 6:aa62e6650559 288
s1588141 5:4b2ff2a4664a 289
s1588141 6:aa62e6650559 290 EMG_R = bqc_R.step(Right.read());
s1588141 6:aa62e6650559 291 EMG_R= fabs(EMG_R);
s1588141 7:075ba23f3147 292 EMG_R= bq3_R.step( EMG_R)*10;
s1588141 6:aa62e6650559 293 scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope
s1588141 6:aa62e6650559 294 if (EMG_R < 0.3){
s1588141 6:aa62e6650559 295 EMG_R=0;
s1588141 6:aa62e6650559 296 } else {
s1588141 6:aa62e6650559 297 EMG_R= 1.0;
s1588141 6:aa62e6650559 298 }
s1588141 5:4b2ff2a4664a 299
s1588141 5:4b2ff2a4664a 300 }
s1588141 8:656b0c493a45 301 //------------------------------------------------------Postition reset----------------------------------
s1588141 8:656b0c493a45 302 //After the robot has been set in it's equlibrium state, this data is used to prevent the robot of destroying itself.
s1588141 6:aa62e6650559 303
s1588141 8:656b0c493a45 304 void Position_reset(){
s1588141 8:656b0c493a45 305 Position_R =0;
s1588141 8:656b0c493a45 306 Position_L =0;
s1588141 8:656b0c493a45 307 }
s1588141 8:656b0c493a45 308
s1588141 8:656b0c493a45 309 //-------------------------------------------------------Limit-------------------------------------
s1588141 8:656b0c493a45 310 //calculates the angele of the robot with for its limit, and checks of its limit has been reached
s1588141 8:656b0c493a45 311
s1588141 8:656b0c493a45 312 int Limit(){
s1588141 8:656b0c493a45 313 double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm
s1588141 8:656b0c493a45 314 int limit = 0;
s1588141 8:656b0c493a45 315 if (fabs(angle) < -13*2*pi*136/(33*360) ){ //limit angle of -13 degrees translated to the motor
s1588141 8:656b0c493a45 316 limit = 1;
s1588141 8:656b0c493a45 317 }
s1588141 8:656b0c493a45 318 if (angle > 15*2*pi*136/(33*360) ){ //limit angle of 15 degrees tranlated to the motor
s1588141 8:656b0c493a45 319 limit = 2;
s1588141 8:656b0c493a45 320 }
s1588141 8:656b0c493a45 321 return limit;
s1588141 8:656b0c493a45 322
s1588141 8:656b0c493a45 323
s1588141 8:656b0c493a45 324 }
s1588141 6:aa62e6650559 325 /////////////-----------------------Booting---------------------------------------=
s1588141 6:aa62e6650559 326 void Boot(){
s1588141 6:aa62e6650559 327
s1588141 6:aa62e6650559 328 //Fucnction that initializes variables that have to be initalized within the main of the script
s1588141 6:aa62e6650559 329 //And does the same thing for functions like encoder reset
s1588141 6:aa62e6650559 330 pc.baud(115200);
s1588141 6:aa62e6650559 331 pc.printf("\r\n**BOARD RESET**\r\n");
s1588141 6:aa62e6650559 332
s1588141 6:aa62e6650559 333 M_L_S.period(1.0/Motor_Frequency);
s1588141 6:aa62e6650559 334 M_L_S = 0.0;
s1588141 6:aa62e6650559 335 M_R_S.period(1.0/Motor_Frequency);
s1588141 6:aa62e6650559 336 M_R_S= 0.0;
s1588141 6:aa62e6650559 337 rotation = 1;
s1588141 6:aa62e6650559 338 translation = 0;
s1588141 6:aa62e6650559 339 Encoder_L.reset();
s1588141 6:aa62e6650559 340 Encoder_R.reset();
s1588141 6:aa62e6650559 341
s1588141 6:aa62e6650559 342 Grap = 1;
s1588141 6:aa62e6650559 343 grip1 = 1;
s1588141 6:aa62e6650559 344
s1588141 8:656b0c493a45 345 bqc_L.add( &bq1_L ).add( &bq2_L );
s1588141 6:aa62e6650559 346 bqc_R.add(&bq1_R).add(&bq2_R);
s1588141 6:aa62e6650559 347 bqc_Change.add(&bq1_Change).add(&bq2_Change);
s1588141 6:aa62e6650559 348 Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency); Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
s1588141 6:aa62e6650559 349 Tick_Movement.attach(Set_movement_Flag, 0.25);
s1588141 6:aa62e6650559 350 OnOff.fall(Start_Stop);
s1588141 7:075ba23f3147 351
s1588141 8:656b0c493a45 352 int Maxmin;
s1588141 6:aa62e6650559 353 }
s1588141 6:aa62e6650559 354
s1588141 6:aa62e6650559 355
s1588141 6:aa62e6650559 356
s1588141 0:a2db8cc5d5df 357 int main()
s1588141 0:a2db8cc5d5df 358 {
s1588141 4:8b1df22779a7 359 //---------------------Setting constants and system booting---------------------
s1588141 4:8b1df22779a7 360
s1588141 0:a2db8cc5d5df 361 Boot();
s1588141 6:aa62e6650559 362
s1588141 4:8b1df22779a7 363 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 364 //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 365 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 366 while (true) {
s1588141 6:aa62e6650559 367 if (Sample_Flag == true){
s1588141 4:8b1df22779a7 368 Sample();
s1588141 6:aa62e6650559 369 getEMG();
s1588141 6:aa62e6650559 370 // EMG_Change = getEMG( bqc.step(Change.read()));
s1588141 5:4b2ff2a4664a 371 Signal = pi*(EMG_R-EMG_L);
s1588141 7:075ba23f3147 372 scope.set(3,Signal);//------------------------------------------------------------scope
s1588141 5:4b2ff2a4664a 373
s1588141 5:4b2ff2a4664a 374 scope.send();
s1588141 4:8b1df22779a7 375 Sample_Flag = false;
s1588141 4:8b1df22779a7 376 }
s1588141 4:8b1df22779a7 377 //Main statement that states if the system is active or not
s1588141 4:8b1df22779a7 378 if (Active == true){
s1588141 2:0954eb04bb4c 379 green = 0;
s1588141 4:8b1df22779a7 380 red = 1;
s1588141 4:8b1df22779a7 381 Change_Movement();
s1588141 4:8b1df22779a7 382 if (Controller_Flag == true){
s1588141 4:8b1df22779a7 383 switch (movement) {
s1588141 4:8b1df22779a7 384 case 0:
s1588141 4:8b1df22779a7 385 //Clockwise rotation of the robot
s1588141 4:8b1df22779a7 386 direction_L = -1;
s1588141 4:8b1df22779a7 387 direction_R = -1;
s1588141 6:aa62e6650559 388 rotation = 1;
s1588141 6:aa62e6650559 389 translation = 0;
s1588141 4:8b1df22779a7 390 break;
s1588141 4:8b1df22779a7 391
s1588141 4:8b1df22779a7 392 case 1:
s1588141 4:8b1df22779a7 393 //Radial movement outwards for a positive value
s1588141 4:8b1df22779a7 394 direction_L = 1;
s1588141 4:8b1df22779a7 395 direction_R = -1;
s1588141 6:aa62e6650559 396 rotation = 0;
s1588141 6:aa62e6650559 397 translation = 1;
s1588141 4:8b1df22779a7 398 break;
s1588141 4:8b1df22779a7 399 }
s1588141 8:656b0c493a45 400 switch (Limit()){ //setting limit
s1588141 8:656b0c493a45 401 case 0:
s1588141 8:656b0c493a45 402 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 8:656b0c493a45 403 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 8:656b0c493a45 404 break;
s1588141 8:656b0c493a45 405 case 1:
s1588141 8:656b0c493a45 406 if (Signal < 0){
s1588141 8:656b0c493a45 407 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 8:656b0c493a45 408 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 8:656b0c493a45 409 }
s1588141 8:656b0c493a45 410 break;
s1588141 8:656b0c493a45 411 case 2:
s1588141 8:656b0c493a45 412 if (Signal > 0){
s1588141 8:656b0c493a45 413 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 8:656b0c493a45 414 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 8:656b0c493a45 415 }
s1588141 8:656b0c493a45 416 break;
s1588141 8:656b0c493a45 417 }
s1588141 4:8b1df22779a7 418 Controller_Flag = false;
s1588141 4:8b1df22779a7 419 }
s1588141 1:d8bb72c9c019 420 } else {
s1588141 4:8b1df22779a7 421 if (Active == false){
s1588141 4:8b1df22779a7 422 green = 1;
s1588141 4:8b1df22779a7 423 red = 0;
s1588141 4:8b1df22779a7 424 M_R_S = 0;
s1588141 1:d8bb72c9c019 425 M_L_S = 0;
s1588141 4:8b1df22779a7 426 }
s1588141 2:0954eb04bb4c 427 }
s1588141 0:a2db8cc5d5df 428 }
s1588141 4:8b1df22779a7 429 }
s1588141 4:8b1df22779a7 430