ECE 4333 / Mbed 2 deprecated Lab3

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Lab3.cpp@6:1d16cc833e0d, 2016-02-26 (annotated)

Committer:
A_Sterner
Date:
Fri Feb 26 21:55:31 2016 +0000
Revision:
6:1d16cc833e0d
Parent:
5:e33e69d4eecf
Child:
7:241bde733699
Dicks;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
A_Sterner 0:b3cd4463d972 1 // EE4333 Robotics Lab 3
A_Sterner 0:b3cd4463d972 2
A_Sterner 0:b3cd4463d972 3 // Library Imports
A_Sterner 0:b3cd4463d972 4
A_Sterner 0:b3cd4463d972 5 //#include "InterruptIn.h"
A_Sterner 0:b3cd4463d972 6 //#include "rtos.h"
A_Sterner 0:b3cd4463d972 7 #include "mbed.h"
A_Sterner 0:b3cd4463d972 8 #include "Serial.h"
A_Sterner 0:b3cd4463d972 9 #include "stdio.h"
A_Sterner 0:b3cd4463d972 10
A_Sterner 0:b3cd4463d972 11 // Function Declarations
A_Sterner 0:b3cd4463d972 12
A_Sterner 2:82e4eac97f0a 13 void DE0_Init(int);
A_Sterner 2:82e4eac97f0a 14 void L_MotorInit(void);
A_Sterner 2:82e4eac97f0a 15 void R_MotorInit(void);
A_Sterner 5:e33e69d4eecf 16 signed int InputLeft(void);
A_Sterner 5:e33e69d4eecf 17 signed int InputRight(void);
A_Sterner 2:82e4eac97f0a 18 void ControlThread(void);
A_Sterner 2:82e4eac97f0a 19 int SaturateAdd(int x, int y);
A_Sterner 2:82e4eac97f0a 20 float SaturateLimit(float x, float limit);
A_Sterner 2:82e4eac97f0a 21 signed int SignExtend(signed int x);
A_Sterner 0:b3cd4463d972 22
A_Sterner 0:b3cd4463d972 23 // ********************************************************************
A_Sterner 0:b3cd4463d972 24 // GLOBAL VARIABLE DECLARATIONS
A_Sterner 0:b3cd4463d972 25 // ********************************************************************
A_Sterner 0:b3cd4463d972 26
A_Sterner 6:1d16cc833e0d 27 signed int R_setpoint; // Desired Angular Speed ( rad/sec )
A_Sterner 6:1d16cc833e0d 28 signed int L_setpoint;
A_Sterner 4:43aef502bb73 29 float R_e; // Velocity Error
A_Sterner 4:43aef502bb73 30 float R_u; // Control Signal
A_Sterner 4:43aef502bb73 31 float L_e;
A_Sterner 4:43aef502bb73 32 float L_u;
A_Sterner 2:82e4eac97f0a 33 int L_integrator; // Left Integrator State
A_Sterner 4:43aef502bb73 34 int R_integrator;
A_Sterner 2:82e4eac97f0a 35 signed int dPositionLeft; // DE0 Register 0
A_Sterner 4:43aef502bb73 36 signed int dPositionRight;
A_Sterner 2:82e4eac97f0a 37 int dTimeLeft; // DE0 Register 1
A_Sterner 4:43aef502bb73 38 int dTimeRight;
A_Sterner 2:82e4eac97f0a 39
A_Sterner 0:b3cd4463d972 40 // *********************************************************************
A_Sterner 0:b3cd4463d972 41 // PROCESSES AND THREADS
A_Sterner 0:b3cd4463d972 42 // *********************************************************************
A_Sterner 0:b3cd4463d972 43
A_Sterner 0:b3cd4463d972 44
A_Sterner 0:b3cd4463d972 45 // *********************************************************************
A_Sterner 0:b3cd4463d972 46 // PIN DECLARATIONS
A_Sterner 0:b3cd4463d972 47 // *********************************************************************
A_Sterner 0:b3cd4463d972 48
A_Sterner 2:82e4eac97f0a 49 // Digital I/O Pins
A_Sterner 0:b3cd4463d972 50
A_Sterner 0:b3cd4463d972 51 DigitalOut led1(LED1); // Thread Indicators
A_Sterner 0:b3cd4463d972 52 DigitalOut led2(LED2); //
A_Sterner 0:b3cd4463d972 53 DigitalOut led3(LED3); //
A_Sterner 0:b3cd4463d972 54 DigitalOut led4(LED4); //
A_Sterner 0:b3cd4463d972 55
A_Sterner 0:b3cd4463d972 56 DigitalOut DirL(p29); // Direction of Left Motor
A_Sterner 0:b3cd4463d972 57 DigitalOut DirR(p30); // Direction of Right Motor
A_Sterner 0:b3cd4463d972 58
A_Sterner 0:b3cd4463d972 59 // SPI Related Digital I/O Pins
A_Sterner 0:b3cd4463d972 60
A_Sterner 0:b3cd4463d972 61 DigitalOut SpiReset(p11);
A_Sterner 0:b3cd4463d972 62 DigitalOut IoReset(p12);
A_Sterner 0:b3cd4463d972 63
A_Sterner 2:82e4eac97f0a 64 //PWM
A_Sterner 0:b3cd4463d972 65
A_Sterner 0:b3cd4463d972 66 PwmOut PwmL(p22);
A_Sterner 0:b3cd4463d972 67 PwmOut PwmR(p21);
A_Sterner 0:b3cd4463d972 68
A_Sterner 2:82e4eac97f0a 69 //Serial
A_Sterner 0:b3cd4463d972 70
A_Sterner 0:b3cd4463d972 71 Serial pc(USBTX, USBRX); // tx and rx for PC serial channel via USB cable
A_Sterner 6:1d16cc833e0d 72 Serial Bluetooth(p9,p10); // Pins tx(p9) , rx(p10) for bluetooth serial channel
A_Sterner 0:b3cd4463d972 73
A_Sterner 2:82e4eac97f0a 74 //SPI
A_Sterner 0:b3cd4463d972 75
A_Sterner 0:b3cd4463d972 76 SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
A_Sterner 2:82e4eac97f0a 77
A_Sterner 2:82e4eac97f0a 78 //Interrupts
A_Sterner 2:82e4eac97f0a 79
A_Sterner 0:b3cd4463d972 80 Ticker ControlInterrupt; // Internal Interrupt to trigger Control Thread
A_Sterner 0:b3cd4463d972 81
A_Sterner 0:b3cd4463d972 82
A_Sterner 0:b3cd4463d972 83 // ***************************************************
A_Sterner 0:b3cd4463d972 84 // DE0 Init
A_Sterner 0:b3cd4463d972 85 // ***************************************************
A_Sterner 0:b3cd4463d972 86
A_Sterner 0:b3cd4463d972 87 void DE0_Init(int SpiControlWord){
A_Sterner 0:b3cd4463d972 88
A_Sterner 0:b3cd4463d972 89 int mode = 1;
A_Sterner 0:b3cd4463d972 90 int bits = 16;
A_Sterner 0:b3cd4463d972 91
A_Sterner 0:b3cd4463d972 92 DE0.format(bits,mode);
A_Sterner 0:b3cd4463d972 93
A_Sterner 0:b3cd4463d972 94 // Verify Peripheral ID
A_Sterner 0:b3cd4463d972 95
A_Sterner 0:b3cd4463d972 96 // Generates single square pulse to reset DE0 IO
A_Sterner 0:b3cd4463d972 97
A_Sterner 0:b3cd4463d972 98 IoReset = 0;
A_Sterner 0:b3cd4463d972 99 IoReset = 1;
A_Sterner 0:b3cd4463d972 100 IoReset = 0;
A_Sterner 0:b3cd4463d972 101
A_Sterner 0:b3cd4463d972 102 // Generates single square pulse to reset DE0 SPI
A_Sterner 0:b3cd4463d972 103
A_Sterner 0:b3cd4463d972 104 SpiReset = 0;
A_Sterner 0:b3cd4463d972 105 SpiReset = 1;
A_Sterner 0:b3cd4463d972 106 SpiReset = 0;
A_Sterner 0:b3cd4463d972 107
A_Sterner 0:b3cd4463d972 108 // Writes to DE0 Control Register
A_Sterner 0:b3cd4463d972 109
A_Sterner 0:b3cd4463d972 110 int ID = DE0.write(SpiControlWord); // SPI Control Word specifies SPI settings
A_Sterner 0:b3cd4463d972 111
A_Sterner 0:b3cd4463d972 112 if(ID == 23){ // DE0 ID 23 (0x0017)
A_Sterner 6:1d16cc833e0d 113 Bluetooth.printf("\n\r >> DE0 Initialized.\n\r");}
A_Sterner 0:b3cd4463d972 114 else{
A_Sterner 6:1d16cc833e0d 115 Bluetooth.printf("\n\r >> Failed to initialize DE0 board.\n\r");}
A_Sterner 0:b3cd4463d972 116 }
A_Sterner 0:b3cd4463d972 117
A_Sterner 0:b3cd4463d972 118 // ***************************************************
A_Sterner 0:b3cd4463d972 119 // Left Motor Initialization
A_Sterner 0:b3cd4463d972 120 // ***************************************************
A_Sterner 0:b3cd4463d972 121
A_Sterner 0:b3cd4463d972 122 // Pwm Pin Left Motor : p21
A_Sterner 0:b3cd4463d972 123 // Direction Pin Left Motor : p29
A_Sterner 0:b3cd4463d972 124
A_Sterner 0:b3cd4463d972 125 void L_MotorInit(void){
A_Sterner 0:b3cd4463d972 126
A_Sterner 4:43aef502bb73 127 DirL = 1; // Defaults to 1
A_Sterner 0:b3cd4463d972 128
A_Sterner 0:b3cd4463d972 129 // Direction bit logic output
A_Sterner 0:b3cd4463d972 130 // 0 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 131 // 1 : Forwards ( Advance )
A_Sterner 0:b3cd4463d972 132
A_Sterner 0:b3cd4463d972 133 PwmL.period_us(100);
A_Sterner 4:43aef502bb73 134 PwmL.write(0);
A_Sterner 0:b3cd4463d972 135
A_Sterner 0:b3cd4463d972 136 }
A_Sterner 0:b3cd4463d972 137
A_Sterner 0:b3cd4463d972 138 // ***************************************************
A_Sterner 0:b3cd4463d972 139 // Right Motor Initialization
A_Sterner 0:b3cd4463d972 140 // ***************************************************
A_Sterner 0:b3cd4463d972 141
A_Sterner 0:b3cd4463d972 142 // Pwm Pin Right Motor : p22
A_Sterner 0:b3cd4463d972 143 // Direction Pin Right Motor : p30
A_Sterner 0:b3cd4463d972 144
A_Sterner 0:b3cd4463d972 145 void R_MotorInit(void){
A_Sterner 0:b3cd4463d972 146
A_Sterner 4:43aef502bb73 147 DirR = 0; // Defaults to 0.
A_Sterner 0:b3cd4463d972 148
A_Sterner 0:b3cd4463d972 149 // Direction bit logic output
A_Sterner 2:82e4eac97f0a 150 // 0 : Forwards ( Advance )
A_Sterner 2:82e4eac97f0a 151 // 1 : Backwards ( Reverse )
A_Sterner 0:b3cd4463d972 152
A_Sterner 0:b3cd4463d972 153 PwmR.period_us(100);
A_Sterner 2:82e4eac97f0a 154 PwmR.write(0);
A_Sterner 0:b3cd4463d972 155
A_Sterner 0:b3cd4463d972 156 }
A_Sterner 0:b3cd4463d972 157
A_Sterner 0:b3cd4463d972 158 /// ***************************************************
A_Sterner 5:e33e69d4eecf 159 // User Input Left
A_Sterner 0:b3cd4463d972 160 // ***************************************************
A_Sterner 0:b3cd4463d972 161
A_Sterner 5:e33e69d4eecf 162 signed int InputLeft(void){
A_Sterner 0:b3cd4463d972 163
A_Sterner 2:82e4eac97f0a 164 signed int input;
A_Sterner 0:b3cd4463d972 165
A_Sterner 6:1d16cc833e0d 166 Bluetooth.printf("\n\r Please enter a desired angular speed for the left motor (rad/sec) >> ");
A_Sterner 6:1d16cc833e0d 167 Bluetooth.scanf("%i",&input);
A_Sterner 5:e33e69d4eecf 168
A_Sterner 6:1d16cc833e0d 169 Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
A_Sterner 5:e33e69d4eecf 170
A_Sterner 5:e33e69d4eecf 171 return input;
A_Sterner 5:e33e69d4eecf 172
A_Sterner 5:e33e69d4eecf 173 }
A_Sterner 5:e33e69d4eecf 174
A_Sterner 5:e33e69d4eecf 175 /// ***************************************************
A_Sterner 5:e33e69d4eecf 176 // User Input Right
A_Sterner 5:e33e69d4eecf 177 // ***************************************************
A_Sterner 5:e33e69d4eecf 178
A_Sterner 5:e33e69d4eecf 179 signed int InputRight(void){
A_Sterner 5:e33e69d4eecf 180
A_Sterner 5:e33e69d4eecf 181 signed int input;
A_Sterner 5:e33e69d4eecf 182
A_Sterner 6:1d16cc833e0d 183 Bluetooth.printf("\n\r Please enter a desired angular speed for the right motor (rad/sec) >> ");
A_Sterner 6:1d16cc833e0d 184 Bluetooth.scanf("%i",&input);
A_Sterner 5:e33e69d4eecf 185
A_Sterner 6:1d16cc833e0d 186 Bluetooth.printf("\n\r Your setpoint is >> %i\n\r",input);
A_Sterner 0:b3cd4463d972 187
A_Sterner 2:82e4eac97f0a 188 return input;
A_Sterner 0:b3cd4463d972 189
A_Sterner 0:b3cd4463d972 190 }
A_Sterner 0:b3cd4463d972 191
A_Sterner 0:b3cd4463d972 192
A_Sterner 2:82e4eac97f0a 193 /// ***************************************************
A_Sterner 2:82e4eac97f0a 194 // Control Thread
A_Sterner 2:82e4eac97f0a 195 // ***************************************************
A_Sterner 2:82e4eac97f0a 196
A_Sterner 2:82e4eac97f0a 197 void ControlThread(void){
A_Sterner 2:82e4eac97f0a 198
A_Sterner 2:82e4eac97f0a 199 // Read Incremental Position from DE0 QEI
A_Sterner 0:b3cd4463d972 200
A_Sterner 2:82e4eac97f0a 201 int dummy = 0x0000; // Pushes dummy information which DE0 ignores, store return from QEI register
A_Sterner 2:82e4eac97f0a 202
A_Sterner 2:82e4eac97f0a 203 dPositionLeft = SignExtend(DE0.write(dummy));
A_Sterner 2:82e4eac97f0a 204 dTimeLeft = DE0.write(dummy);
A_Sterner 4:43aef502bb73 205 dPositionRight = SignExtend(DE0.write(dummy));
A_Sterner 4:43aef502bb73 206 dTimeRight = DE0.write(dummy);
A_Sterner 2:82e4eac97f0a 207
A_Sterner 2:82e4eac97f0a 208 // Computer Angular Speed and Angular Speed Error
A_Sterner 2:82e4eac97f0a 209
A_Sterner 2:82e4eac97f0a 210 signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
A_Sterner 4:43aef502bb73 211 signed int AngularSpeedRight = (123*dPositionRight)/dTimeRight;
A_Sterner 2:82e4eac97f0a 212
A_Sterner 4:43aef502bb73 213 L_e = L_setpoint - AngularSpeedLeft;
A_Sterner 4:43aef502bb73 214 R_e = R_setpoint - AngularSpeedRight;
A_Sterner 4:43aef502bb73 215
A_Sterner 4:43aef502bb73 216 float Kp_L = 2.5;
A_Sterner 4:43aef502bb73 217 float Ki_L = 0.010;
A_Sterner 2:82e4eac97f0a 218
A_Sterner 4:43aef502bb73 219 float Kp_R = 2.5;
A_Sterner 4:43aef502bb73 220 float Ki_R = 0.010;
A_Sterner 2:82e4eac97f0a 221
A_Sterner 4:43aef502bb73 222
A_Sterner 5:e33e69d4eecf 223 if(abs(SaturateLimit((Kp_L*L_e+Ki_L*L_integrator)/35,1))<1){
A_Sterner 4:43aef502bb73 224 L_integrator = L_integrator +L_e;}
A_Sterner 2:82e4eac97f0a 225 else{
A_Sterner 2:82e4eac97f0a 226 L_integrator = L_integrator;
A_Sterner 0:b3cd4463d972 227 }
A_Sterner 2:82e4eac97f0a 228
A_Sterner 5:e33e69d4eecf 229 if(abs(SaturateLimit((Kp_R*R_e+Ki_R*R_integrator)/35,1))<1){
A_Sterner 4:43aef502bb73 230 R_integrator = R_integrator +R_e;}
A_Sterner 4:43aef502bb73 231 else{
A_Sterner 4:43aef502bb73 232 R_integrator = R_integrator;
A_Sterner 4:43aef502bb73 233 }
A_Sterner 5:e33e69d4eecf 234
A_Sterner 4:43aef502bb73 235 L_u = SaturateLimit( (Kp_L*L_e+Ki_L*L_integrator),1);
A_Sterner 4:43aef502bb73 236 R_u = SaturateLimit( (Kp_R*R_e+Ki_R*R_integrator),1);
A_Sterner 4:43aef502bb73 237
A_Sterner 5:e33e69d4eecf 238 if(L_u <=0)
A_Sterner 5:e33e69d4eecf 239 DirL = 0;
A_Sterner 5:e33e69d4eecf 240 else
A_Sterner 5:e33e69d4eecf 241 DirL = 1;
A_Sterner 5:e33e69d4eecf 242
A_Sterner 5:e33e69d4eecf 243 if(R_u <=0)
A_Sterner 5:e33e69d4eecf 244 DirR = 1;
A_Sterner 5:e33e69d4eecf 245 else
A_Sterner 5:e33e69d4eecf 246 DirR = 0;
A_Sterner 5:e33e69d4eecf 247
A_Sterner 5:e33e69d4eecf 248 PwmL.write(abs(L_u));
A_Sterner 5:e33e69d4eecf 249 PwmR.write(abs(R_u));
A_Sterner 0:b3cd4463d972 250 }
A_Sterner 0:b3cd4463d972 251
A_Sterner 2:82e4eac97f0a 252 /// ***************************************************
A_Sterner 2:82e4eac97f0a 253 // SaturateAdd
A_Sterner 2:82e4eac97f0a 254 // ***************************************************
A_Sterner 2:82e4eac97f0a 255
A_Sterner 2:82e4eac97f0a 256 signed int SaturateAdd(signed int x, signed int y){
A_Sterner 2:82e4eac97f0a 257
A_Sterner 2:82e4eac97f0a 258 signed int z = x + y;
A_Sterner 2:82e4eac97f0a 259
A_Sterner 2:82e4eac97f0a 260 if( (x>0) && (y>0)&& (z<=0) ){
A_Sterner 2:82e4eac97f0a 261 z = 0x7FFFFFFF;}
A_Sterner 2:82e4eac97f0a 262
A_Sterner 2:82e4eac97f0a 263 else if( (x<0)&&(y<0)&&(z>=0) ){
A_Sterner 2:82e4eac97f0a 264 z = 0x80000000;}
A_Sterner 2:82e4eac97f0a 265
A_Sterner 2:82e4eac97f0a 266 return z;
A_Sterner 2:82e4eac97f0a 267 }
A_Sterner 2:82e4eac97f0a 268
A_Sterner 2:82e4eac97f0a 269 /// ***************************************************
A_Sterner 2:82e4eac97f0a 270 // SaturateLimit
A_Sterner 2:82e4eac97f0a 271 // ***************************************************
A_Sterner 2:82e4eac97f0a 272
A_Sterner 2:82e4eac97f0a 273 float SaturateLimit(float x, float limit){
A_Sterner 2:82e4eac97f0a 274
A_Sterner 2:82e4eac97f0a 275 if (x > limit){
A_Sterner 2:82e4eac97f0a 276 return limit;}
A_Sterner 2:82e4eac97f0a 277
A_Sterner 2:82e4eac97f0a 278 else if(x < -limit){
A_Sterner 2:82e4eac97f0a 279 return(-limit);}
A_Sterner 2:82e4eac97f0a 280
A_Sterner 2:82e4eac97f0a 281 else{
A_Sterner 2:82e4eac97f0a 282 return x;}
A_Sterner 2:82e4eac97f0a 283
A_Sterner 2:82e4eac97f0a 284 }
A_Sterner 2:82e4eac97f0a 285
A_Sterner 2:82e4eac97f0a 286 /// ***************************************************
A_Sterner 2:82e4eac97f0a 287 // Sign Extend
A_Sterner 2:82e4eac97f0a 288 // ***************************************************
A_Sterner 2:82e4eac97f0a 289
A_Sterner 2:82e4eac97f0a 290 signed int SignExtend(int x){
A_Sterner 2:82e4eac97f0a 291
A_Sterner 2:82e4eac97f0a 292 if(x&0x00008000){
A_Sterner 2:82e4eac97f0a 293 x = x|0xFFFF0000;
A_Sterner 2:82e4eac97f0a 294 }
A_Sterner 2:82e4eac97f0a 295
A_Sterner 2:82e4eac97f0a 296 return x;
A_Sterner 2:82e4eac97f0a 297 }
A_Sterner 2:82e4eac97f0a 298
A_Sterner 2:82e4eac97f0a 299
A_Sterner 2:82e4eac97f0a 300
A_Sterner 2:82e4eac97f0a 301 // ==============================================================================================================
A_Sterner 2:82e4eac97f0a 302 // ==============================================================================================================
A_Sterner 2:82e4eac97f0a 303
A_Sterner 2:82e4eac97f0a 304
A_Sterner 2:82e4eac97f0a 305 // *********************************************************************
A_Sterner 2:82e4eac97f0a 306 // MAIN FUNCTION
A_Sterner 2:82e4eac97f0a 307 // *********************************************************************
A_Sterner 2:82e4eac97f0a 308
A_Sterner 2:82e4eac97f0a 309 int main(){
A_Sterner 2:82e4eac97f0a 310
A_Sterner 2:82e4eac97f0a 311 // Initialization
A_Sterner 2:82e4eac97f0a 312
A_Sterner 4:43aef502bb73 313 DE0_Init(0x8004);
A_Sterner 2:82e4eac97f0a 314 L_MotorInit();
A_Sterner 4:43aef502bb73 315 R_MotorInit();
A_Sterner 2:82e4eac97f0a 316 L_integrator = 0;
A_Sterner 4:43aef502bb73 317 R_integrator = 0;
A_Sterner 2:82e4eac97f0a 318 ControlInterrupt.attach(&ControlThread, 0.0005);
A_Sterner 2:82e4eac97f0a 319
A_Sterner 2:82e4eac97f0a 320 // Specify Setpoint ( rads/sec )
A_Sterner 2:82e4eac97f0a 321
A_Sterner 5:e33e69d4eecf 322 L_setpoint = InputLeft();
A_Sterner 5:e33e69d4eecf 323 R_setpoint = InputRight();
A_Sterner 4:43aef502bb73 324
A_Sterner 2:82e4eac97f0a 325 // Display Global Variables to Console
A_Sterner 6:1d16cc833e0d 326 Bluetooth.printf("\n\r ========= LEFT ======= ========= RIGHT =======");
A_Sterner 6:1d16cc833e0d 327 Bluetooth.printf("\n\r US VE IS CS US VE IS CS ");
A_Sterner 2:82e4eac97f0a 328
A_Sterner 2:82e4eac97f0a 329 while(1){
A_Sterner 2:82e4eac97f0a 330
A_Sterner 4:43aef502bb73 331 float L_error_t = L_e;
A_Sterner 4:43aef502bb73 332 float L_u_t = L_u;
A_Sterner 4:43aef502bb73 333
A_Sterner 4:43aef502bb73 334 float R_error_t = R_e;
A_Sterner 4:43aef502bb73 335 float R_u_t = R_u;
A_Sterner 2:82e4eac97f0a 336
A_Sterner 6:1d16cc833e0d 337
A_Sterner 6:1d16cc833e0d 338 Bluetooth.printf("\n\r %i %2.1f %i %2.2f %i %2.1f %i %2.2f",L_setpoint,L_error_t,L_integrator,L_u_t,R_setpoint,R_error_t,R_integrator,R_u_t);
A_Sterner 2:82e4eac97f0a 339 wait(0.75);
A_Sterner 2:82e4eac97f0a 340 }
A_Sterner 2:82e4eac97f0a 341
A_Sterner 2:82e4eac97f0a 342 }