Koen Boulogne / Mbed 2 deprecated Project

Basis aansturing projectgroep 3

Dependencies:   Biquad HIDScope MODSERIAL QEI mbed

main.cpp@9:e764cb50d343, 2016-11-07 (annotated)

Committer:
s1588141
Date:
Mon Nov 07 13:30:49 2016 +0000
Revision:
9:e764cb50d343
Parent:
8:656b0c493a45 
Final program;

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 7:075ba23f3147 54
s1588141 0:a2db8cc5d5df 55
s1588141 4:8b1df22779a7 56 //--------------------------------------------------------------
s1588141 4:8b1df22779a7 57 //--------------------All Motors--------------------------------
s1588141 4:8b1df22779a7 58 //--------------------------------------------------------------
s1588141 9:e764cb50d343 59 volatile int movement = 1,direction_L =1, direction_R =-1; //determining the direction of the motors
s1588141 4:8b1df22779a7 60
s1588141 4:8b1df22779a7 61 DigitalOut M_L_D(D4); //Richting motor links->
s1588141 4:8b1df22779a7 62 //while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive
s1588141 2:0954eb04bb4c 63 PwmOut M_L_S(D5); //Speed motor links
s1588141 0:a2db8cc5d5df 64 DigitalOut M_R_D(D7); //Richting motor Rechts
s1588141 2:0954eb04bb4c 65 PwmOut M_R_S(D6); //Speed motor Rechts
s1588141 4:8b1df22779a7 66 const int Motor_Frequency = 1000;
s1588141 4:8b1df22779a7 67
s1588141 4:8b1df22779a7 68 DigitalOut Grap(D9); //Graple active or not;
s1588141 4:8b1df22779a7 69
s1588141 4:8b1df22779a7 70 //----------------------Lights ---------------------------------4
s1588141 4:8b1df22779a7 71 DigitalOut translation(D0);
s1588141 4:8b1df22779a7 72 DigitalOut grip1(D1);
s1588141 4:8b1df22779a7 73 DigitalOut rotation(D2);
s1588141 4:8b1df22779a7 74 DigitalOut grip2(D3);
s1588141 0:a2db8cc5d5df 75
s1588141 0:a2db8cc5d5df 76 //--------------------------------------------------------------
s1588141 0:a2db8cc5d5df 77 //-----------------------Functions------------------------------
s1588141 0:a2db8cc5d5df 78 //--------------------------------------------------------------
s1588141 0:a2db8cc5d5df 79
s1588141 4:8b1df22779a7 80 //-----------------------on of sitch of the sytem-----------------------------
s1588141 4:8b1df22779a7 81
s1588141 9:e764cb50d343 82
s1588141 9:e764cb50d343 83
s1588141 9:e764cb50d343 84 AnalogIn Active(A5);
s1588141 9:e764cb50d343 85
s1588141 4:8b1df22779a7 86
s1588141 4:8b1df22779a7 87
s1588141 4:8b1df22779a7 88 //--------------------------------Sampling------------------------------------------
s1588141 5:4b2ff2a4664a 89
s1588141 5:4b2ff2a4664a 90 Ticker Tick_Sample;// ticker for data sampling
s1588141 5:4b2ff2a4664a 91
s1588141 4:8b1df22779a7 92 bool Sample_Flag = false;
s1588141 4:8b1df22779a7 93
s1588141 4:8b1df22779a7 94 void Set_Sample_Flag(){
s1588141 4:8b1df22779a7 95 Sample_Flag = true;
s1588141 0:a2db8cc5d5df 96 }
s1588141 9:e764cb50d343 97
s1588141 4:8b1df22779a7 98 void Sample(){
s1588141 1:d8bb72c9c019 99
s1588141 4:8b1df22779a7 100 //This function reads out the position and the speed of the motors in radiants
s1588141 4:8b1df22779a7 101 //Aand stores them in the values Speed and Position
s1588141 4:8b1df22779a7 102
s1588141 2:0954eb04bb4c 103 //Position in Radials:
s1588141 2:0954eb04bb4c 104 Previous_Position_L = Position_L;
s1588141 2:0954eb04bb4c 105 Previous_Position_R = Position_R;
s1588141 2:0954eb04bb4c 106 Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
s1588141 2:0954eb04bb4c 107 Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
s1588141 2:0954eb04bb4c 108 //Speed in RadPers second
s1588141 4:8b1df22779a7 109 Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
s1588141 4:8b1df22779a7 110 Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
s1588141 4:8b1df22779a7 111
s1588141 4:8b1df22779a7 112 Encoder_L.reset();
s1588141 4:8b1df22779a7 113 Encoder_R.reset();
s1588141 4:8b1df22779a7 114
s1588141 4:8b1df22779a7 115 }
s1588141 4:8b1df22779a7 116
s1588141 4:8b1df22779a7 117 bool Controller_Flag = false;
s1588141 4:8b1df22779a7 118
s1588141 4:8b1df22779a7 119 Ticker Tick_Controller;
s1588141 4:8b1df22779a7 120
s1588141 4:8b1df22779a7 121 void Set_controller_Flag(){
s1588141 4:8b1df22779a7 122 Controller_Flag = true;
s1588141 4:8b1df22779a7 123 }
s1588141 5:4b2ff2a4664a 124
s1588141 5:4b2ff2a4664a 125
s1588141 4:8b1df22779a7 126
s1588141 5:4b2ff2a4664a 127 //-------------------------------PI ---------------------------------
s1588141 4:8b1df22779a7 128
s1588141 5:4b2ff2a4664a 129 double PI(double e, const double Kp, const double Ki, double Sf,
s1588141 5:4b2ff2a4664a 130 double &e_int){
s1588141 4:8b1df22779a7 131 //This function is a PID controller that returns the errorsignal for the plant
s1588141 4:8b1df22779a7 132
s1588141 4:8b1df22779a7 133
s1588141 4:8b1df22779a7 134 e_int = e_int + Sf * e;
s1588141 4:8b1df22779a7 135 // PID
s1588141 5:4b2ff2a4664a 136 return Kp*e + Ki*e_int ;
s1588141 1:d8bb72c9c019 137 }
s1588141 4:8b1df22779a7 138
s1588141 4:8b1df22779a7 139 //---------------------------Controller---------------------------------------
s1588141 4:8b1df22779a7 140
s1588141 4:8b1df22779a7 141
s1588141 4:8b1df22779a7 142
s1588141 4:8b1df22779a7 143 // Controller gains (motor1−Kp,−Ki,...)
s1588141 8:656b0c493a45 144 const double Kpm= 0.1, Kim = 0.005;
s1588141 8:656b0c493a45 145 double e_L_int = 0;
s1588141 5:4b2ff2a4664a 146
s1588141 4:8b1df22779a7 147 double Controller_L(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 148 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 149 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 150
s1588141 4:8b1df22779a7 151 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 152 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 153
s1588141 4:8b1df22779a7 154 //PID controller
s1588141 8:656b0c493a45 155 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_L_int );
s1588141 4:8b1df22779a7 156 //Determining rotational direction of the motor
s1588141 8:656b0c493a45 157 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 158 M_L_D = 1;
s1588141 4:8b1df22779a7 159 } else {
s1588141 8:656b0c493a45 160 M_L_D = 0;
s1588141 4:8b1df22779a7 161 }
s1588141 9:e764cb50d343 162 if (fabs(Speed_Set)< 1){
s1588141 8:656b0c493a45 163 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
s1588141 8:656b0c493a45 164 return 0;
s1588141 8:656b0c493a45 165 } else {
s1588141 4:8b1df22779a7 166 return fabs(Speed_Set);
s1588141 8:656b0c493a45 167 }
s1588141 8:656b0c493a45 168 } else {
s1588141 9:e764cb50d343 169 return 1;
s1588141 8:656b0c493a45 170 }
s1588141 8:656b0c493a45 171 }
s1588141 8:656b0c493a45 172 double e_R_int = 0;
s1588141 4:8b1df22779a7 173 double Controller_R(int direction, double signal, double reference ){
s1588141 4:8b1df22779a7 174 //This funcition returns a value that will be sent to drive the motor
s1588141 4:8b1df22779a7 175 // - Motor_Direction_Input is the Digital in of the motor
s1588141 4:8b1df22779a7 176
s1588141 4:8b1df22779a7 177 // 1 for dirrection means that a clockwise rotation wil be regarded as positive
s1588141 4:8b1df22779a7 178 // and -1 for direction means the opposite
s1588141 4:8b1df22779a7 179
s1588141 5:4b2ff2a4664a 180 //PI controller
s1588141 8:656b0c493a45 181 double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_R_int);
s1588141 4:8b1df22779a7 182 //Determining rotational direction of the motor
s1588141 4:8b1df22779a7 183 if ((reference-signal*direction >= 0)){
s1588141 4:8b1df22779a7 184 M_R_D = 0;
s1588141 4:8b1df22779a7 185 } else {
s1588141 4:8b1df22779a7 186 M_R_D = 1;
s1588141 4:8b1df22779a7 187 }
s1588141 9:e764cb50d343 188 if (fabs(Speed_Set)< 1){
s1588141 5:4b2ff2a4664a 189 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
s1588141 5:4b2ff2a4664a 190 return 0;
s1588141 5:4b2ff2a4664a 191 } else {
s1588141 4:8b1df22779a7 192 return fabs(Speed_Set);
s1588141 5:4b2ff2a4664a 193 }
s1588141 4:8b1df22779a7 194 } else {
s1588141 9:e764cb50d343 195 return 1;
s1588141 4:8b1df22779a7 196
s1588141 4:8b1df22779a7 197 }
s1588141 4:8b1df22779a7 198 }
s1588141 4:8b1df22779a7 199 //-----------------------------Change_Movement----------------------------------
s1588141 4:8b1df22779a7 200 // This function changes the movement type of the robot as a consequence of the input by EMG
s1588141 4:8b1df22779a7 201 // Or when the EMG signal is hold for 1.5S The grapler wil activate
s1588141 4:8b1df22779a7 202 Ticker Tick_Movement;
s1588141 4:8b1df22779a7 203 int check = 0;
s1588141 4:8b1df22779a7 204
s1588141 4:8b1df22779a7 205 bool Movement_Flag = false;
s1588141 4:8b1df22779a7 206 void Set_movement_Flag(){
s1588141 4:8b1df22779a7 207 Movement_Flag = true;
s1588141 4:8b1df22779a7 208 }
s1588141 4:8b1df22779a7 209
s1588141 4:8b1df22779a7 210 void Change_Movement(){
s1588141 4:8b1df22779a7 211 if (Movement_Flag == true){
s1588141 4:8b1df22779a7 212 if (EMG_Change > 0.5f ){
s1588141 4:8b1df22779a7 213 check += 1;
s1588141 9:e764cb50d343 214 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 215 grip1 = 1;
s1588141 4:8b1df22779a7 216 }
s1588141 9:e764cb50d343 217 if (check > 6){
s1588141 4:8b1df22779a7 218 grip2 =1;
s1588141 4:8b1df22779a7 219 }
s1588141 4:8b1df22779a7 220 } else {
s1588141 4:8b1df22779a7 221 //Hold the signal shorter than 1.5 seconds
s1588141 4:8b1df22779a7 222 if (check <= 6 and check > 1 ){
s1588141 4:8b1df22779a7 223 movement = !movement;
s1588141 4:8b1df22779a7 224 grip1 = 1;
s1588141 4:8b1df22779a7 225 }
s1588141 4:8b1df22779a7 226 // Hold the signal longer than 1.5 seconds
s1588141 4:8b1df22779a7 227 if (check > 6){
s1588141 4:8b1df22779a7 228 grip2 =1;
s1588141 4:8b1df22779a7 229 Grap = !Grap;
s1588141 4:8b1df22779a7 230
s1588141 4:8b1df22779a7 231 }
s1588141 4:8b1df22779a7 232
s1588141 4:8b1df22779a7 233 grip2 = 0;
s1588141 4:8b1df22779a7 234 grip1 =0;
s1588141 4:8b1df22779a7 235 check = 0;
s1588141 4:8b1df22779a7 236
s1588141 4:8b1df22779a7 237 }
s1588141 4:8b1df22779a7 238 Movement_Flag = false;
s1588141 4:8b1df22779a7 239 }
s1588141 2:0954eb04bb4c 240 }
s1588141 5:4b2ff2a4664a 241 //-----------------------------EMG Signal determinator----------------------
s1588141 5:4b2ff2a4664a 242 BiQuadChain bqc_L;
s1588141 5:4b2ff2a4664a 243 BiQuadChain bqc_R;
s1588141 6:aa62e6650559 244 BiQuadChain bqc_Change;
s1588141 5:4b2ff2a4664a 245 BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 246 BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 247 BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 248
s1588141 5:4b2ff2a4664a 249 BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 5:4b2ff2a4664a 250 BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 5:4b2ff2a4664a 251 BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 5:4b2ff2a4664a 252
s1588141 6:aa62e6650559 253
s1588141 6:aa62e6650559 254 BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
s1588141 6:aa62e6650559 255 BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
s1588141 6:aa62e6650559 256 BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
s1588141 9:e764cb50d343 257 //HIDScope scope(5); // Sending two sets of data
s1588141 9:e764cb50d343 258 HIDScope scope(3);
s1588141 6:aa62e6650559 259 void getEMG(){
s1588141 6:aa62e6650559 260
s1588141 5:4b2ff2a4664a 261
s1588141 6:aa62e6650559 262 EMG_Change = bqc_Change.step(Change.read());
s1588141 6:aa62e6650559 263 EMG_Change = fabs(EMG_Change);
s1588141 9:e764cb50d343 264 EMG_Change = bq3_Change.step(EMG_Change)*30;
s1588141 7:075ba23f3147 265 scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope
s1588141 9:e764cb50d343 266 //scope.send(); // scope.set(4,Change.read());
s1588141 9:e764cb50d343 267 if (EMG_Change < 0.4){
s1588141 6:aa62e6650559 268 EMG_Change=0;
s1588141 6:aa62e6650559 269 } else {
s1588141 6:aa62e6650559 270 EMG_Change=1.0;
s1588141 6:aa62e6650559 271 }
s1588141 6:aa62e6650559 272
s1588141 6:aa62e6650559 273
s1588141 6:aa62e6650559 274 EMG_L = bqc_L.step(Left.read());
s1588141 6:aa62e6650559 275 EMG_L= fabs(EMG_L);
s1588141 9:e764cb50d343 276 EMG_L= bq3_L.step( EMG_L)*30;
s1588141 7:075ba23f3147 277 scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope
s1588141 7:075ba23f3147 278
s1588141 9:e764cb50d343 279 if (EMG_L < 0.5){
s1588141 6:aa62e6650559 280 EMG_L=0;
s1588141 6:aa62e6650559 281 } else {
s1588141 9:e764cb50d343 282 //EMG_L=1.0;
s1588141 6:aa62e6650559 283 }
s1588141 6:aa62e6650559 284
s1588141 6:aa62e6650559 285
s1588141 5:4b2ff2a4664a 286
s1588141 6:aa62e6650559 287 EMG_R = bqc_R.step(Right.read());
s1588141 6:aa62e6650559 288 EMG_R= fabs(EMG_R);
s1588141 9:e764cb50d343 289 EMG_R= bq3_R.step( EMG_R)*30;
s1588141 9:e764cb50d343 290 scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope
s1588141 9:e764cb50d343 291 if (EMG_R < 0.5){
s1588141 6:aa62e6650559 292 EMG_R=0;
s1588141 6:aa62e6650559 293 } else {
s1588141 9:e764cb50d343 294 //EMG_R= 1.0;
s1588141 6:aa62e6650559 295 }
s1588141 9:e764cb50d343 296 scope.send();
s1588141 5:4b2ff2a4664a 297 }
s1588141 8:656b0c493a45 298 //------------------------------------------------------Postition reset----------------------------------
s1588141 8:656b0c493a45 299 //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 300
s1588141 8:656b0c493a45 301 void Position_reset(){
s1588141 8:656b0c493a45 302 Position_R =0;
s1588141 8:656b0c493a45 303 Position_L =0;
s1588141 8:656b0c493a45 304 }
s1588141 8:656b0c493a45 305
s1588141 8:656b0c493a45 306 //-------------------------------------------------------Limit-------------------------------------
s1588141 8:656b0c493a45 307 //calculates the angele of the robot with for its limit, and checks of its limit has been reached
s1588141 9:e764cb50d343 308 int config_count = 0;
s1588141 9:e764cb50d343 309 double max_angle = 0;
s1588141 9:e764cb50d343 310 double min_angle = 0.5;
s1588141 8:656b0c493a45 311 int Limit(){
s1588141 8:656b0c493a45 312 double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm
s1588141 8:656b0c493a45 313 int limit = 0;
s1588141 9:e764cb50d343 314 if (angle < min_angle){ //limit angle of -13 degrees translated to the motor -13*2*pi*136/(33*360)
s1588141 8:656b0c493a45 315 limit = 1;
s1588141 8:656b0c493a45 316 }
s1588141 9:e764cb50d343 317 if (angle > max_angle){ //limit angle of 15 degrees tranlated to the motor 2*15*2*pi*136/(33*360)
s1588141 8:656b0c493a45 318 limit = 2;
s1588141 8:656b0c493a45 319 }
s1588141 8:656b0c493a45 320 return limit;
s1588141 8:656b0c493a45 321
s1588141 8:656b0c493a45 322
s1588141 8:656b0c493a45 323 }
s1588141 9:e764cb50d343 324
s1588141 9:e764cb50d343 325
s1588141 9:e764cb50d343 326
s1588141 9:e764cb50d343 327 bool Configure_Flag = true;
s1588141 9:e764cb50d343 328
s1588141 9:e764cb50d343 329
s1588141 6:aa62e6650559 330 /////////////-----------------------Booting---------------------------------------=
s1588141 6:aa62e6650559 331 void Boot(){
s1588141 6:aa62e6650559 332
s1588141 6:aa62e6650559 333 //Fucnction that initializes variables that have to be initalized within the main of the script
s1588141 6:aa62e6650559 334 //And does the same thing for functions like encoder reset
s1588141 6:aa62e6650559 335 pc.baud(115200);
s1588141 6:aa62e6650559 336 pc.printf("\r\n**BOARD RESET**\r\n");
s1588141 6:aa62e6650559 337
s1588141 6:aa62e6650559 338 M_L_S.period(1.0/Motor_Frequency);
s1588141 6:aa62e6650559 339 M_L_S = 0.0;
s1588141 6:aa62e6650559 340 M_R_S.period(1.0/Motor_Frequency);
s1588141 6:aa62e6650559 341 M_R_S= 0.0;
s1588141 9:e764cb50d343 342 rotation = 0;
s1588141 9:e764cb50d343 343 translation = 1;
s1588141 6:aa62e6650559 344 Encoder_L.reset();
s1588141 6:aa62e6650559 345 Encoder_R.reset();
s1588141 6:aa62e6650559 346
s1588141 9:e764cb50d343 347 Grap = 0;
s1588141 9:e764cb50d343 348 grip1 = 0;
s1588141 6:aa62e6650559 349
s1588141 8:656b0c493a45 350 bqc_L.add( &bq1_L ).add( &bq2_L );
s1588141 6:aa62e6650559 351 bqc_R.add(&bq1_R).add(&bq2_R);
s1588141 6:aa62e6650559 352 bqc_Change.add(&bq1_Change).add(&bq2_Change);
s1588141 9:e764cb50d343 353 Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);
s1588141 9:e764cb50d343 354 Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
s1588141 6:aa62e6650559 355 Tick_Movement.attach(Set_movement_Flag, 0.25);
s1588141 6:aa62e6650559 356 }
s1588141 6:aa62e6650559 357
s1588141 6:aa62e6650559 358
s1588141 6:aa62e6650559 359
s1588141 0:a2db8cc5d5df 360 int main()
s1588141 0:a2db8cc5d5df 361 {
s1588141 4:8b1df22779a7 362 //---------------------Setting constants and system booting---------------------
s1588141 4:8b1df22779a7 363
s1588141 0:a2db8cc5d5df 364 Boot();
s1588141 6:aa62e6650559 365
s1588141 4:8b1df22779a7 366 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 367 //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 368 //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
s1588141 4:8b1df22779a7 369 while (true) {
s1588141 6:aa62e6650559 370 if (Sample_Flag == true){
s1588141 9:e764cb50d343 371
s1588141 4:8b1df22779a7 372 Sample();
s1588141 6:aa62e6650559 373 getEMG();
s1588141 6:aa62e6650559 374 // EMG_Change = getEMG( bqc.step(Change.read()));
s1588141 5:4b2ff2a4664a 375 Signal = pi*(EMG_R-EMG_L);
s1588141 5:4b2ff2a4664a 376
s1588141 9:e764cb50d343 377
s1588141 9:e764cb50d343 378 //Signal = pi*(Right.read()-Left.read());
s1588141 9:e764cb50d343 379 //EMG_Change = Change.read();
s1588141 9:e764cb50d343 380 // scope.set(3,Signal);//------------------------------------------------------------scope
s1588141 9:e764cb50d343 381
s1588141 9:e764cb50d343 382 // scope.send();
s1588141 4:8b1df22779a7 383 Sample_Flag = false;
s1588141 9:e764cb50d343 384 }
s1588141 9:e764cb50d343 385 //-------------------------------------------- Configureren-------------------------------------
s1588141 9:e764cb50d343 386 if (Configure_Flag == true){
s1588141 9:e764cb50d343 387
s1588141 9:e764cb50d343 388 if (config_count < 20){
s1588141 9:e764cb50d343 389 Signal = pi;
s1588141 9:e764cb50d343 390 } else {
s1588141 9:e764cb50d343 391 Signal = -pi;
s1588141 9:e764cb50d343 392 }
s1588141 9:e764cb50d343 393 if (config_count == 19){
s1588141 9:e764cb50d343 394 Position_reset();
s1588141 9:e764cb50d343 395
s1588141 9:e764cb50d343 396 }
s1588141 9:e764cb50d343 397 if (config_count > 40){
s1588141 9:e764cb50d343 398 Configure_Flag = false;
s1588141 9:e764cb50d343 399 max_angle = Position_R-Position_L-0.5;
s1588141 9:e764cb50d343 400 }
s1588141 9:e764cb50d343 401
s1588141 9:e764cb50d343 402
s1588141 9:e764cb50d343 403 M_L_S = 0.02*Controller_L(direction_L,Signal,Speed_L);
s1588141 9:e764cb50d343 404 M_R_S = 0.02*Controller_R(direction_R,Signal,Speed_R);
s1588141 9:e764cb50d343 405
s1588141 9:e764cb50d343 406
s1588141 9:e764cb50d343 407 //Signal = -pi;
s1588141 9:e764cb50d343 408 // M_L_S = 0.03*Controller_L(direction_L,Signal,Speed_L);
s1588141 9:e764cb50d343 409 // M_R_S = 0.03*Controller_R(direction_R,Signal,Speed_R);
s1588141 9:e764cb50d343 410
s1588141 9:e764cb50d343 411
s1588141 9:e764cb50d343 412 Grap = 0;
s1588141 9:e764cb50d343 413 if (Movement_Flag == true){
s1588141 9:e764cb50d343 414 rotation = !rotation;
s1588141 9:e764cb50d343 415 translation =!translation;
s1588141 9:e764cb50d343 416
s1588141 9:e764cb50d343 417 grip1 = !grip1;
s1588141 9:e764cb50d343 418 grip2 = !grip2;
s1588141 9:e764cb50d343 419 config_count ++;
s1588141 9:e764cb50d343 420 Movement_Flag= false;
s1588141 9:e764cb50d343 421 }
s1588141 9:e764cb50d343 422 } else {
s1588141 4:8b1df22779a7 423 //Main statement that states if the system is active or not
s1588141 9:e764cb50d343 424 if (Active.read() > 0.5){
s1588141 2:0954eb04bb4c 425 green = 0;
s1588141 4:8b1df22779a7 426 red = 1;
s1588141 4:8b1df22779a7 427 Change_Movement();
s1588141 9:e764cb50d343 428
s1588141 4:8b1df22779a7 429 if (Controller_Flag == true){
s1588141 4:8b1df22779a7 430 switch (movement) {
s1588141 4:8b1df22779a7 431 case 0:
s1588141 4:8b1df22779a7 432 //Clockwise rotation of the robot
s1588141 9:e764cb50d343 433 direction_L = 1;
s1588141 9:e764cb50d343 434 direction_R = 1;
s1588141 6:aa62e6650559 435 rotation = 1;
s1588141 6:aa62e6650559 436 translation = 0;
s1588141 9:e764cb50d343 437 Signal = Signal*2;
s1588141 9:e764cb50d343 438 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 9:e764cb50d343 439 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 4:8b1df22779a7 440 break;
s1588141 4:8b1df22779a7 441
s1588141 4:8b1df22779a7 442 case 1:
s1588141 4:8b1df22779a7 443 //Radial movement outwards for a positive value
s1588141 4:8b1df22779a7 444 direction_L = 1;
s1588141 4:8b1df22779a7 445 direction_R = -1;
s1588141 6:aa62e6650559 446 rotation = 0;
s1588141 6:aa62e6650559 447 translation = 1;
s1588141 9:e764cb50d343 448
s1588141 9:e764cb50d343 449 switch (Limit()){ //making sure the limits are not passed
s1588141 9:e764cb50d343 450 case 0: //case the robot is in its working space
s1588141 9:e764cb50d343 451 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 9:e764cb50d343 452 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 9:e764cb50d343 453 break;
s1588141 9:e764cb50d343 454 case 1: //case the robot has reached it's lowest limit
s1588141 9:e764cb50d343 455 if (Signal < 0){
s1588141 9:e764cb50d343 456 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 9:e764cb50d343 457 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 9:e764cb50d343 458 } else {
s1588141 9:e764cb50d343 459 M_L_S = 0;
s1588141 9:e764cb50d343 460 M_R_S = 0;
s1588141 9:e764cb50d343 461 }
s1588141 9:e764cb50d343 462 break;
s1588141 9:e764cb50d343 463 case 2: //case the robot has reachted its highest limit
s1588141 9:e764cb50d343 464 if (Signal > 0){
s1588141 9:e764cb50d343 465 M_L_S = Controller_L(direction_L,Signal,Speed_L);
s1588141 9:e764cb50d343 466 M_R_S = Controller_R(direction_R,Signal,Speed_R);
s1588141 9:e764cb50d343 467 } else {
s1588141 9:e764cb50d343 468 M_L_S = 0;
s1588141 9:e764cb50d343 469 M_R_S = 0;
s1588141 9:e764cb50d343 470 }
s1588141 9:e764cb50d343 471 break;
s1588141 9:e764cb50d343 472 }
s1588141 4:8b1df22779a7 473 break;
s1588141 4:8b1df22779a7 474 }
s1588141 9:e764cb50d343 475
s1588141 4:8b1df22779a7 476 Controller_Flag = false;
s1588141 4:8b1df22779a7 477 }
s1588141 1:d8bb72c9c019 478 } else {
s1588141 9:e764cb50d343 479 if (Active.read() < 0.5){
s1588141 9:e764cb50d343 480 green = 1;
s1588141 4:8b1df22779a7 481 red = 0;
s1588141 4:8b1df22779a7 482 M_R_S = 0;
s1588141 1:d8bb72c9c019 483 M_L_S = 0;
s1588141 9:e764cb50d343 484 Grap=0;
s1588141 4:8b1df22779a7 485 }
s1588141 2:0954eb04bb4c 486 }
s1588141 0:a2db8cc5d5df 487 }
s1588141 4:8b1df22779a7 488 }
s1588141 9:e764cb50d343 489 }
s1588141 4:8b1df22779a7 490