From 54196dd83133134f83dc417aae8918f56534b350 Mon Sep 17 00:00:00 2001 From: Armin Date: Sun, 4 Apr 2021 11:46:57 +0200 Subject: [PATCH] Using IMU data --- .github/workflows/TestCompile.yml | 37 +- README.md | 22 +- pictures/WIN_20161218_15_08_55_Pro.jpg | Bin 186326 -> 0 bytes src/ADCUtils.cpp | 142 +++-- src/ADCUtils.h | 12 +- src/AutonomousDrive.cpp | 43 +- src/AutonomousDrivePage.cpp | 49 +- src/BTSensorDrivePage.cpp | 178 +++--- src/CarMotorControl.cpp | 615 ------------------ src/CarMotorControl.h | 162 ----- src/CarPWMMotorControl.cpp | 828 +++++++++++++++++++++++++ src/CarPWMMotorControl.h | 206 ++++++ src/DebugLevel.h | 52 ++ src/Distance.cpp | 84 ++- src/Distance.h | 19 +- src/EncoderMotor.cpp | 640 ++++++++++++------- src/EncoderMotor.h | 123 ++-- src/HCSR04.cpp | 5 +- src/HomePage.cpp | 54 +- src/IMUCarData.cpp | 539 ++++++++++++++++ src/IMUCarData.h | 158 +++++ src/LightweightServo.cpp | 220 +++++++ src/LightweightServo.h | 78 +++ src/LongUnion.h | 94 +++ src/MPU6050Defines.h | 428 +++++++++++++ src/PWMDcMotor.cpp | 656 ++++++++++++-------- src/PWMDcMotor.h | 302 +++++---- src/PathInfoPage.cpp | 6 +- src/RobotCar.h | 65 +- src/RobotCarBlueDisplay.ino | 100 +-- src/RobotCarCommonGui.cpp | 477 ++++++++------ src/RobotCarGui.h | 47 +- src/TestPage.cpp | 135 +++- src/digitalWriteFast.h | 105 ++-- 34 files changed, 4642 insertions(+), 2039 deletions(-) delete mode 100644 pictures/WIN_20161218_15_08_55_Pro.jpg delete mode 100644 src/CarMotorControl.cpp delete mode 100644 src/CarMotorControl.h create mode 100644 src/CarPWMMotorControl.cpp create mode 100644 src/CarPWMMotorControl.h create mode 100644 src/DebugLevel.h create mode 100644 src/IMUCarData.cpp create mode 100644 src/IMUCarData.h create mode 100644 src/LightweightServo.cpp create mode 100644 src/LightweightServo.h create mode 100644 src/LongUnion.h create mode 100644 src/MPU6050Defines.h diff --git a/.github/workflows/TestCompile.yml b/.github/workflows/TestCompile.yml index 82b288c..e1e5d2d 100644 --- a/.github/workflows/TestCompile.yml +++ b/.github/workflows/TestCompile.yml @@ -34,7 +34,6 @@ jobs: build-properties: RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DUSE_ADAFRUIT_MOTOR_SHIELD @@ -44,7 +43,6 @@ jobs: RobotCarBlueDisplay: -DUSE_LAYOUT_FOR_NANO -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DCAR_HAS_PAN_SERVO @@ -58,7 +56,6 @@ jobs: build-properties: RobotCarBlueDisplay: -DBLUETOOTH_BAUD_RATE=BAUD_115200 - -DUSE_STANDARD_SERIAL -DUSE_US_SENSOR_1_PIN_MODE -DDISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN -DCAR_HAS_TOF_DISTANCE_SENSOR @@ -76,22 +73,20 @@ jobs: ref: master path: CustomLibrary_BD # must contain string "Custom" - - name: Checkout new PWMMotorControl library - uses: actions/checkout@v2 - with: - repository: ArminJo/PWMMotorControl - ref: master - path: CustomLibrary_PWM # must contain string "Custom" +# Currently included in source directory +# - name: Checkout new PWMMotorControl library +# uses: actions/checkout@v2 +# with: +# repository: ArminJo/PWMMotorControl +# ref: master +# path: CustomLibrary_PWM # must contain string "Custom" - # Use the arduino-test-compile script, because it is faster - - name: Compile all examples using the bash script arduino-test-compile.sh - env: - # Passing parameters to the script by setting the appropriate ENV_* variables. - ENV_ARDUINO_BOARD_FQBN: ${{ matrix.arduino-boards-fqbn }} - ENV_REQUIRED_LIBRARIES: ${{ env.REQUIRED_LIBRARIES }} - ENV_EXAMPLES_BUILD_PROPERTIES: ${{ toJson(matrix.build-properties) }} - run: | - wget --quiet https://raw.githubusercontent.com/ArminJo/arduino-test-compile/master/arduino-test-compile.sh - ls -l arduino-test-compile.sh - chmod +x arduino-test-compile.sh - ./arduino-test-compile.sh + - name: Compile all examples using the arduino-test-compile action + uses: ArminJo/arduino-test-compile@master + with: + arduino-board-fqbn: ${{ matrix.arduino-boards-fqbn }} + arduino-platform: ${{ matrix.arduino-platform }} + platform-url: ${{ matrix.platform-url }} + required-libraries: ${{ env.REQUIRED_LIBRARIES }} + sketches-exclude: ${{ matrix.sketches-exclude }} + build-properties: ${{ toJson(matrix.build-properties) }} diff --git a/README.md b/README.md index 00ea452..d1c7dd6 100644 --- a/README.md +++ b/README.md @@ -13,9 +13,9 @@ Just overwrite the function doUserCollisionDetection() to test your own skill. You may also overwrite the function fillAndShowForwardDistancesInfo(), if you use your own scanning method. -# Compile options / macros +# Compile options / macros for this software To customize the software to different car extensions, there are some compile options / macros available.
-Modify it by commenting them out or in, or change the values if applicable. Or define the macro with the -D compiler option for gobal compile (the latter is not possible with the Arduino IDE, so consider to use [Sloeber](https://eclipse.baeyens.it).
+Modify it by commenting them out or in, or change the values if applicable. Or define the macro with the -D compiler option for global compile (the latter is not possible with the Arduino IDE, so consider using [Sloeber](https://eclipse.baeyens.it).
Compile options for the used **PWMMotorControl library** like `USE_ENCODER_MOTOR_CONTROL` are described [here](https://github.com/ArminJo/PWMMotorControl#compile-options--macros-for-this-library). | Option | Default | File | Description | @@ -25,22 +25,24 @@ Compile options for the used **PWMMotorControl library** like `USE_ENCODER_MOTOR | `USE_US_SENSOR_1_PIN_MODE` | disabled | RobotCar.h | Use modified HC-SR04 modules or HY-SRF05 ones.
Modify HC-SR04 by connecting 10kOhm between echo and trigger and then use only trigger pin. | | `CAR_HAS_IR_DISTANCE_SENSOR` | disabled | RobotCar.h | Use Sharp GP2Y0A21YK / 1080 IR distance sensor. | | `CAR_HAS_TOF_DISTANCE_SENSOR` | disabled | RobotCar.h | Use VL53L1X TimeOfFlight distance sensor. | -| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. | +| `DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN` | disabled | Distance.h | The distance servo is mounted head down to detect even small obstacles. The Servo direction is reverse then. | | `CAR_HAS_CAMERA` | disabled | RobotCar.h | Enables the `Camera` button for the `PIN_CAMERA_SUPPLY_CONTROL` pin. | | `CAR_HAS_LASER` | disabled | RobotCar.h | Enables the `Laser` button for the `PIN_LASER_OUT` / `LED_BUILTIN` pin. | | `CAR_HAS_PAN_SERVO` | disabled | RobotCar.h | Enables the pan slider for the `PanServo` at the `PIN_PAN_SERVO` pin. | -| `CAR_HAS_TILT_SERVO` | disabled | RobotCar.h | Enables the tilt slider for the `TiltServo` at the `PIN_TILT_SERVO` pin.. | -| `MONITOR_LIPO_VOLTAGE` | disabled | RobotCar.h | Shows VIN voltage and monitors it for undervoltage. Requires 2 additional resistors at pin A2. | -| `VIN_VOLTAGE_CORRECTION` | undefined | RobotCar.h | Voltage to be subtracted from VIN voltage. E.g. if there is a series diode between LIPO and VIN set it to 0.8. | +| `CAR_HAS_TILT_SERVO` | disabled | RobotCar.h | Enables the tilt slider for the `TiltServo` at the `PIN_TILT_SERVO` pin. | +| `ENABLE_RTTTL` | undefined | RobotCar.h | Plays melody after initial timeout has reached. Enables the Melody button, which plays a random melody. | +| `MONITOR_VIN_VOLTAGE` | disabled | RobotCar.h | Shows VIN voltage and monitors it for undervoltage. VIN/11 at A2, 1MOhm to VIN, 100kOhm to ground. | +| `VIN_VOLTAGE_CORRECTION` | undefined or 0.8 for UNO | RobotCar.h | Voltage to be subtracted from VIN voltage for voltage monitoring. E.g. if there is a series diode between LIPO and VIN as on the UNO boards, set it to 0.8. | +| `SUPPORT_EEPROM_STORAGE` | disabled | RobotCar.h | Activates the buttons to store compensation and drive speed. | -### Modifying library properties with Arduino IDE -First use *Sketch/Show Sketch Folder (Ctrl+K)*.
+### Modifying compile options with Arduino IDE +First, use *Sketch > Show Sketch Folder (Ctrl+K)*.
If you did not yet stored the example as your own sketch, then you are instantly in the right library folder.
Otherwise you have to navigate to the parallel `libraries` folder and select the library you want to access.
In both cases the library files itself are located in the `src` directory.
-### Modifying library properties with Sloeber IDE -If you are using Sloeber as your IDE, you can easily define global symbols with *Properties/Arduino/CompileOptions*.
+### Modifying compile options with Sloeber IDE +If you are using Sloeber as your IDE, you can easily define global symbols with *Properties > Arduino > CompileOptions*.
![Sloeber settings](https://github.com/ArminJo/ServoEasing/blob/master/pictures/SloeberDefineSymbols.png) # Wall detection diff --git a/pictures/WIN_20161218_15_08_55_Pro.jpg b/pictures/WIN_20161218_15_08_55_Pro.jpg deleted file mode 100644 index 502e2931be71c6e0d905ff8b705a5370f3f50a8d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 186326 zcmeFYc{rQh+cuicgQ~WQnyRg$C>}$khVrzEqNp*%R5d0Msd?sk+M+0Gt|)3ABGeE; zgx1(1)D%M`YKod?p$T@L_j}*pyN~bq{@B0$$3BjIyj{ti&nsS z(GS2ST`e6gz_DY;0MA%2z|qvTMNNOGGXS8k50C%=0A~Rwk6i(rU|GjlFF+&|aPmKF zz**flBL8JyYFQKjoMBbIz&fVJ+5iHJtdFz*Sij3}27WW}n}Od9{AS=c1OFo#IQ8xX z;5ZBURRG}9`*$b*WtSx;{D*z=o+1#CnE2oJf2)bJZ~tNJx0?7LdENX5{hNW`4E$!` zHv_*J_!k3mf5<8-%gQOs-n$_SRQ}_hG7t#(KkZ}v;Mjlbh)YBNH1JzT{ENol#%~6G zGw_>%-wga_;D0CsM^ooMx_W!RP?nK#_mZ}EL_T+tc0eLz{Ow=J$V&eq15j1>e_`(c zck;gR+{p#%eqU@2-z;_m>Udua4AlQa|AmH=EA(-or;|ybfvH0v+(F4vOkIurysE#l zKjH<#$=m*hKf=x3OWFVat^a6UnPva8TjrLUs;8r~vhgFW|E$6KcK_CYj>XT)xv_MYi~$uMbF7)Mp+f2*e|^UuKkZv$2PUwc^C zS=PUpV$pWv|AX?olYfik-*o+^>)&GF->Urk>iSLBzs116Rr&YT_5U-v{#$K1xw9me zA4^LekpUWj6UUGL^JbNktoNz2r%s(bdFuR`GpEm9IDg>++j%xN_KR08v0uD=k&W#V z_od66SGl;jE^zQ%fBX!HukIkb~*YCxP12b_fvoq z$8G_RUp{u?^0A{ffDkLjbc&Tb^H2KBf0kp%Sz|nX=IpuiY^(~{OMv6YPMkP?^29%* zX4MX19S5AeeCo-$Jft4An?`e@QBDaQPGJ> z$tkI6@6%DadHDr}Ma3nhRn;}vT3lUyLrZI0dq?NjZ(Rd}L&Kzz(XsKV>6zI%+WgOj z#f{Ca?VVlv-u|zDa2;de{IBxAfcyEWptWo!*SzMCI#9>SeR^Yh@h0cAN)kJ(^X{F z95fAutnNhX`$#(8a)P9-g)Dl8tU`MTTP8hHxQE7G2G;#V+mFoGxtTaWhC+ru_rOmh z>I`_zq@IEbolK1M@<#2>Ydn z-_~@O!`<_mc0hZjx)On-b(PKGK*B=z!-~vXMf5m{#B;8}UPpjasvBkp*)2zabN+H) zlWp90%B64H;VUJl4wQ)sDrb{a(lvuRnO8ZdX9MXS?`!@R24+W2F;rhv(EF&c{jM1c zhEw+YQ2o|$*tU{c;ytAdL~d4zKSsFhPwA~X(vy+n*Y$$YCWOw^G94S@Q;njGj{QT& zH7ncIuds!n_5dtAqti4_9>*9iaDe-HTok!!7AR%u{|py<{1gOR^i{D|-}qHvr4n@Y z)+_RR|Kb7tia3x_G0fJQ_=$;49gmprgWTdt$l{4fvx+-!B}(J&*j-w zj9P8pYSK=qi?90vSi|{Gas!rc#?Ovmot0gBTWX7xyYxMfW6ixBSw7U zFs_PoDTkp9^u;Z69E$Q5xizzjg}Mf+m4rBOO#o^P;JI8S*tN?NQW&;|mJBQNimo{7Qyx@{Fk|Wkm+vDd`;_5+#WR-3irHc8UOIPOKNF%ni( zv~-$3TrWEzgE1_`Rt?fafmDbsLLbcCQ0>j+6p@g!dH3<_GY3LS`|#``3~{i!lTJ#z zpVd|@8b2U2YPkDXc3Z0YdKs3Ht+qIR5L~pA;_AwXYdXDE@C73mtBY9 zsR?b-?rwx84{b)m3JzXgEw~nU_*DJPDs!o)e09~=s39gsV$jUbi*@_>*oVHEKoTGK z@FeL{;E{VJ<#1(WWR^O|BeCJaePQ#J>}d>ia5rwG^6l(YZIOu&#&WdWUHbXoVGDWF zB}H!)0u*OVYWAC)kV^2cjWX*prXva!ppHP&*%_nq!3x(Yj}@J6|*TchfoSu)COefDi0T>b2R70;9S;;VC3mAt9rU zw|5v72f3fFV-^CQ_F0(5NNXMe-i0@<(SIU2pe%?&NBnLN&9r^!aM9n>F+mmIFjSTuhgq&N38bACh$ zy23IhO|h&YK^L;uKdGwq*F=>;{Ez?VIiqk0R|2 zl~$8ms)59^(s`I%Go(HKM||tSvdeeXgH|C0q9snKy6><2+T;gC6XQ(>{WT^BT`3YQ zj*b9!w?4QaZpZ6=8pZ%ch}br-U;79Y&h}VEu*iriPw|6te-jIoMUD@2d0t&scen4t z(h1vvfqBpBkde(^u#ZGSe7pVv?Vi_UYS`aJnvzBh^>fk@9nx*Qk}2hDnz#05#WnKqo!!u`rzkrh)tP}MYTD#3UZXYh}ehOy!HMWKmTWX(6bJB&3cVKhFXZA|l+eTf)r2;<9IKg{a3pHF!I3s+@aai`A>H=Kj7XRP%CDpJF(#)AT71+a~M6*q=k`FLi2(N9kR@o7KrU|c%?GP>%q zn%y+TTEo_0vIDBtFDcE2(#=n5a4PlCGxcfkd!yJ`nLzsmgfx&|r+oOw92)l*eXLwI6!-1sS|P_a|C!9;_qw6ao8&4-ev8PxNG3EBsYbt%N+XXtLr>V63?&hw6QDQNtqMdAKM%Fy7Dby+P1+6?(-cf|_C zthu^9B2_qRC^WA_!KXPBxfk?{Q9nSz?nUnG)NDCej}GV)h`>)+5K<7z%bygs8u&b<5w5s#5y z^C^HmXAVsaO0)*-mq=(8TgELK8U}oH^^qkk_+77|w3i@@PXOhiOL}-(w-KH;zdB$= z6}g@GsogHnL6KI3mG8kQr7Mzt#yw%7dP}J^biPlNZE|uz3DP-CWy!SS?C&!O{cdRq z9-D4bL7!FSX8Wq|~IgvKx`9M*spMffz|EFNAcs}#&Yzcm5DKhx&6`|2qzq$AOi{9 zDO$-fPr=rQz6`EG>=uv>H1~^*Nne^~eEVxNnWmTlwEbd4$fB1~&3CLS=Dfk;kNS3g zR|*c}v_ushR0TPRP)owtU!MoLr);2_Z0<&Sr>w$4#vRknU68sPcSU6O+s&5fb#|BC z4{5fEi+!I_SH~wRGYj`SSJA?)VP`{c2Sd`OgW=Or3i?twWbI!oUD;Egi+we<OgN~YGqRpgl1M=cwUo%Ie(D}0p zyCnBC%kE|bX!VQAdhSJ9FB;yCCAuy^u#K)Q9^+Ncu4%rfi~~UK zAW)r1h1Ou{Oy6sAM0;dc?$!d;lNZw>U3?4oV zLiJ_$-~^?C$-4>}dOkpV`_M)#J4YpjUOaDe=9;c@E{E7bsqc8&as|$pU1fP~%`(V9 z%T9#StWF<9Bu||o5Zi!CfuZ}rudfof`yl;Yu=siKL=-0RXC)7ta__x1Y+bUwG{f7Z{yfzLIr#J7eHdIa zEOsPzD(sS`U}Vfnj-6B23T_8v67Y zo@U3OReBg3%#|n%+Hj18(M#^-M;gaifBH0xm;e$E8f=8c`^+c?6(CI9PH3JLtt?8x zt_D`&&q%MV2_CM&;&Xq_JPpsnyLQl(;D$+F?<{t-EM2L+-j^6!>B zuJsv0NXf#y(ugOeX6}k{wBDrLpZz~~(|91SU7e#uJ9hdc8>)ariE*-JO?20aON4)V zx08W7-q!wOFy~%Q-hwPtyFJim$ovR!+fPoLK}neimfsT!nhN^1x2p~f12+)-^ANEUCBPi#>r1%k_3EnT$ zC3!FWBEj+*`dgq1)tsW8d`sP6no#R-7pBrhK_N) zRGP}QVzlQxa4+RD=#)^qE|$+jM>AY`kT4q%Kj!E@6q0hl9{6(4eUgzJ)&fEvjJGCu zJ0DVP=Tit7I8y{Rwz|Bqc_kUCRXafQj%<)3F5Z`k0;2X-~Ul)qTzK^nW+g=dl^?ye8#f0+iMH<(QsA?#yq}$K}oDH;f3J^iy6>c(vJ+J`3m) z%+VIEi7-?(BMV;KWcZ~(C994AK73U^Nq8-4cjc>H^|a_f2x=M8bbasT#(T{I>m$JD zB!sKRdQqDkLg?AE$rZWi4vSI`u&07WfVVcF_OOP8rP#(E0nW#_UAMj7Q`X$Ng)z&R zLcW=D6TF}9!IZH)aKGYb%+&jZ$D;1ZRMQJMZQoxYltD7PygxAFa}YpAWJtiv`Y6}KP2Us|n!ZN}T2FNM*S3JgoN;3eb% zR5e$E7kHc#Clr_)6gKT<-k<$qc7T?jb<40SK!>k_VX=>vS!vH!iPf&nbHbQT*bMSW zVB^r5q|<9s-^N?BUbn3N4SVSFz{h5v;2yC_6jY#EL zxM9_8IWWa*a7|7QYf^N&JUv@J8XG@Ce$(c6<)gYZR6U}SDO0XJjbvZoE7SI*LbRpl z=4s?D^V-ENk$hC;BtOuX-J0V~@SoZ1Nt(#pZtLRXegfj<`^_=H*&_gFzVh`_oMSl7 zqYjj3Qg|@)7{g2X8K@qaU*|G}aSHMk@)Y|*u{;#pbC@!`1l+>C&x-nVSWD1X^~!?7iT)h*M!V(|E~L#^W`5>OsalrSnoc)3dQWUOt{;eg zT|L}}(#ri3=BIvcf26_+mSFp^N^7dWLGD9IiSaUz@dUnW(2A7cI8mQlXcY@%OI_2Z z!uV;qB|vzM1!=zo|8S?s3x@%%dBPxDd_%fUGIxhah@qOrR>W&33N`Tq@d*-DGTLX} z(Ig%l?RTGpn$sMeN3}}Dcq;CvCbq_~@HYTYEk;E>Oz2Ftf2uJ_eR1xnT$w7qn$9 zwH+0N%IaV!T_JV%;xnf_D9n$jH9fS=S;gK|gMf&KZtmnqe&=TSM`QzXZDwR~at4&_ zXS`B`|5STPdeLsGNJ=GhVo+oe2%h_vle}7@xLR1dGBM~;T{mbYn-Mfnfni0{tqRWB zSpGpH{h-oHC+(~jcB>|=Cp6*5+57IURcgLC0&uG-`Ug#2M2+w@E=es#xZZUL?m7aj zghY;~(fe0HVD+iVF?xp(Cs1ryO=NLX^Mv3uQP2Gd;5SZ)oI-vHV`qw2J{r867c{(| zwP}>9zB&@T?C9*Gl7R<`>(v+Ch2jZzKc4M@zBCB6QZCmJ*bJh(hC6#lyPFvuSTkMv zevoNFWT1jd#(h7ty20sYtRC8N@>chqYmhq4?kV{cGoh%r*LZuTzQ+j+D1gaoH_!WfEXClZ3$cD!r(8PCcnUACFklAgb(-0cFB`CT&z&1T@4At8=&^(V+$(y zaxDel9EC1??L$*S>!laxwYbgj86@Y&;+r5}>xWQN ztm0(Bc(JC&tsK*5YIYeuPjxOJ5|}p7KG8+@wbZKlfsPW%Q7_rV@ZRO8(UxfRdS~l_ zVOuZt(S@iZzfeFHUQ=tsIborCdALvs&yxonU?FOzRHEU%w_~Kh>sE=}jC&}5osUXsPaFzd@@~S-@wxzN8$3?XV++bVHmx9ks5Jl6x%Xkhg_2}ra zR$7h3eVcgY!iz>EvB;VrNVvlwupdrkAKK=&4}#v-ZQdN@wd<{?zeRhcRCoe8!EfUg zJ)mQ;mVyC9l@%htPhn}+_P0N}Z7wAm(2R#|mc9AZ8vCmv)up`s(jG2w8&28V`nY7* z-07V};U$Twif2!K_B#SNp$Fq`>S=lE#^E%BD2kG+23*aJd0d#>1rKsdel`g4Y<%&$ zgFO)m_2akE3WboqC)NuL=ytra+Rvy`v)%c+NCJ6GsMh|~aFb%CJQ9_MYuwD|=f>W3 zjE4Ip3KJ|s7CUsr;K>>K@nYG^TBx-DX#9_!23WkeLX2EiX`MM28QX zH9^)ZMqNxR8xK;M@NXZj@zBYJ+cdjpGeqg@1FqPpu}K!={1+=QDt6Yt-=JopnmkFbpX zqIuQy?48``<39q5`6hsdnvf)`vr*5?W`}Bw%zCIlt{Q1RK9?o=W}{&x>r5X#6(2p# zMAkRPseBmNOVX+#jO8UbE07S~D1Ma^3?2T;#W4t^BohnLaak9h7KoSlg4anAhF+9o zgqqm}GVRvsk+Xw{#Oamv^4t7zYk`c`!}GumNh92bUwEjHrA>y42#(>i*4$gTr1vI`~$60kDR{SQH9)* zsj>URFe95_=EM_}zJIHw6L+x4P2o-7qZq~-*-MAg>80q&+Y*y2<|yin(W(ZpGj(#s z#Gy$_RNpw@ZCpnJ%*iBWQOyc3C|V%$Vcd56-9e5v1vYo+J3I7Y0Mu=voHkLD{h$l` zoQvO&?{77yeRSD+bL)XaTVK<_X#O?IeAgc-t>*a-YdWI~MFyVcm9IMsTY}EQ z2wcrSSmC(bf5^53`dVeJYHoC}Mis#N2HJhOe~V&DnXtB14{5$NlJhM90ju|$=r)yB zeG+q6E<3UY{u`{=M%uH9hoxFxC5%o~6z%am9&|9`&UWu=-umv0!z>#EZKAH~5)^Zz zY1wUHP96Hj;R<3w-f{v;=kN=CUT3)DfeZZc8a*x6HCK_Ll6p}gjd19+52hW=W;FjW zOsiaqtzn}08kJ@5u*9~ilD8zKK3_St%K~5J4;hG{brwbkMYhg$qCkD)(EEsfjmrL>EhoR^q2W@kEJs% zf5kUp$$EmeFR`m$!>uiF)8KB5znE#iBeKS-wr}lD7fB$sz$(GcPBg~J|IlVhLO6`m z6FVQOuxDcLK*G`O*{1wOSA(^9pU*Gj4+AKQJMv=^raSpx+Qfn^VJ zsgFi^GxE$O^!45^)TXLN{#f=yAtQ?h*G$}M=@&}y4I>{{SK)jDR*M&pu`OBMDM-bB zD8JMGWfbI|?{s(QakqQ??fA+|j=0?6hz;M^T-L%s;L8@2g1RbQUpc}@Y%c_!8_R2o z(~w9pJt`eRW^42=3;OxJg(_999U}CP&2icY)}x zB?hMCTXDCzeBvTg{Fi?8MKIkG8*5*ZHjV&uWg0?2|DB!ew)ns33%h63ZhZH`cO`uY z3lDj;g!2LW4`|xohD+e6=Da4>T^kci@N}3*^Tk6YS6f9>T$~Sf$k$!5x=F}yLt-Sq z^MxqOLz!m<2!*P%vZvwME6p7D27qz+fJK2#_E87sB_!}u^*>0fhcHBW} zIatI$@0|F9wVa)YtSpkw{@@2L7|R0Uhb51buU@3er(o`r>rYpf)D~8z3`b_;^(Vyr zf`>?qp}dkZw3nfxf!_tNsHh){(1)KWvF1R`dS#_31irpQ1%dtE@Xv< zWJEtYt(n_|LjK;`a-bEzO}2o4U*kLiTrW}EQB)5MoCoUJ7=p^DYSUJ?43T&u+m^Yp zGd}<8aLHug2iAH)1vNinRUW}cFPT^^pgh9W<*wv$XW$6RtN}?{=cjZSmwxZwGD(5e zf;s)nA0jCFMMnT02oFY5diLDoW)Se!!73E52RsK4>8$#M9!R6*fQJUK_bCxsxKnfm zDK-OZL@|h(zIgM)bz4r+$tm8oJy6grzM4RK|#4Z zR#o745|y_liYF}Nu7Z1o1k_=!Oi;iP;OxjtQ+TLw#%w?y?_>2}JKus$_jaa*VyHKf zCzy)S=2d=qs}6w~Z;@Z-G#?yMDiCuzFf&ZhXjF}CQTnMk)oYJrnJ zNUB@?vYLo?LBj&+Oz;|g-qwhR3IEcFDC#bUnR_if*V)l|^=PAGS+1E1^Zqs08HEK8 zDm-GCz7D9i-gyk(OGnkM@dq2%M;rv7Sznbe^-rCjC6I*o2ZZPxFuJ6b0cv``6q@$glBfq3Q|M07`8m&#uP-YMUiYYfH*t zuT~E4WYDXuySF|$&@+~7H-r`9dXE6|;8)7gc}R!RSapZ14e82rKdP&#M*xZXQND72 zcOhBO42%S%zfrgr>`?fVdaIFf3e)D)R;Kp!AkEKh1kRm-EUa#a%GTj}XH)ReQG*_Z z>3S6@D_hUvn+!_t@ruBg*KA!z z<%qT$>&;U=koyfV6iSj>N4bFLVkOUvlwUBHyJoRP=+aKOE>g>tg>;LK7uck*gS?NE1j25*7wo5RN~)Y><3SeFM?IYBn%5@|56yN- z`tZCdV?_F{FMd&$Y(SW;*Ho7w5uV`h7QM|c;tl#b)$U+>-Tk7n&~=O8KN)-h9^}_M zZl)_6O2byNk*pZxSg^*1-WQZx{Hdk{xPLd(=rbPP-RLwUsm|%HhqzAa5IMXv)5`ZN zI=~iqJ5M2TQ#IY;1(DL;+}LLL#(YVxF`-&muoL0ppJ&>SMXRe(wc-{Gf8lh?-JT)$ zoh8I;Eb?PN9B*D75w#C*?{BR)_ek8I+`fPJy2R_!f|G+gO4{!`8_)`qR3O|KC%dY) zlU83lvxnaLIT(|&9I{wE;ihxD(+?6mE9X~og)$2Z8g1D0vXS^OTk$<3evT7q(&Rv4#c`=CNCm$YCJWuGJC3lSs`F-V?u%X@FJ)EZ^a^${8NW? zpTe4yZP}1`d6(yA?{SG zU~U(QEEqs?4;WhtOfgB<#)cR-Y2Yg&`^$$zQwTj9k0?3luRA03fRuTbX zX}~7<;YRPf#5G)!fmOWB!KY!m;rf;xYI0qVoX4@kKYi`CSM9uRLPE%k;a^}p4K`mA z3eOmb_J<;AEx0#Jm35E-$rX@4ag>+N>$M2r{#nt*dcn zd3qoD1vRfey-}Jrp3;<6?405t{i3up$s7kx4{Nb3l1bqa}}pUp4xZoi!)k$v+O# zoe6%?`OT2NRJnDUiv7$q9Ma03(!=lUXHg&RhhninSNzMw`6o20eq)j)9WsYSSES!q zveulBm3EE?tV~E|q89a{uHv~T0%&z9K;OWueuB~Z5rBF1M7u_t5OxQH%|OOVA*Lr< zKOidD_agI-04|Gz^}Uk~>XeG-dmB?r82E0>5LWEZT5}JrjQrJyBMHGz8|C5dY^&C_ zu95l*3dte!zMec+^pFL244Y-gB_!cOm7Qt(*?6!S``fT*Zxn)RW52uy%a>Xsod1M} zI__#2NRtH89_08;p|3b0m_y$w0a7Tvr9Q0My5c}GdE%-~7a68c_A>#~7NJIda;)WH z>s}jTvUgoZEPOAl<@Ow5Ipyh1%sk`GTLNNsD3(-KDDS z2eyf&!kT9;KOzXfwby2zu0d>1PON!e4r?6lgW2KhrXP0OiA9%0Vb+FRzS|cMHs94h z2Rs?;agAb$LnW6rs|<(Ts|d^sc;V=LmyGVv^>i69DLA|M&f13C9bbz68Z;zLMr8|# z>Mp!>@+pkeOtf8}LuOzM65S7X9ybJ`#f9g}abMKflG?^!{Rw#Vh`Q<1GNRAN*=A~;uAw|bK1+-stH&VKPS=SS*vP^oz(uuPI7yYWm>p#PDa$a0YuI3Nb>#jv zP1DyBZe!oxen=L`tN-q*UVWc3Q#=&>0O?eBFq$(Ny{gbWIL1@}_p<3acDUnkVl|y21jza%L7#g@;wDR}NEEe}^y`W|; z<4{dHC2V83B7O}Ub=Pr=^Kic+wkJiV43Zkz9BxvgyCt&}sLGdu|KfEi$-G-;;$0u=9hEZ1 zv*F$w#CLP%A1Pa24=acbwQoOMkC2N_tiJE^0~sf+D!HN23TZ@EV%GAy9rpAPP8DP4 zZp-P=!Y=%!tIkESdz}i3hu=0XTv?ElLmckzR7_N`6Gb{-GnG_l+%}=LYhnyCH?Bm@ zb)q91ub7ZK31lBgO{lh3BvA}7y((6N^ENt-#W5i2KKLF4i}(YjCIIwS?rXyvM%Z!< z`>=Hb8!Yq0ug`X^kvGvoy6f>{^v;nixqctlwJ!_=hOAO9``ULO-aJ-rN}5+K)yp<% z-q?L57NyFMh9+3^2gMXaZYRKJU3XSA^w(L=EgRF7y&_3*N$R1^q%vz!5xt^~lvOI{ zpZ04ZNKR{|%-I%p=WA`cI|wO**0E-MJ2$o!O0fW3o;h6gCP%OMmjz&KRQvx1dR1b2s}tFUDntnoQyLR)VgtTyArAks9cuO07C8 z4kn>1U(;NXPU}i4lI#qfoDKbJFQ%2H{AN0DU6=gOn6bRk7`cjZV+tJPD8$Fvv9IoM zTv|wjoUSOiRj@XGb4;Kjt`ZcSJ+)#B+Q4@};vyaFy^0wtZKnk+0?SLG)e z1PLlE=^6#X0xu@{`@DfXIB`pyyyaIm$-Bp)y)lfof~9U$-x*IT-XEw6W%F-+8aEwu z-$XYxDCpg96~yqBF>wQ%lOR9gp`TjsO}P*{oE6QcQ2^ zCXC9Lz8_y&v?Ss1^`u1v0_RB*g2Wlq0*ttAt$$qRoVl6|RQV8m4 z95@27D`K#h$KI0H44fuvf~hqm7@P`zv)b^67QywZ=0nTT{gUo z-RXrOU0v&MlO7QAWu46-4mzw`rf2wsiyx#{ALtF_Y^LSgp-@G>k&Vr;T)&W+GCdnpO|+P&|}K zih#ksxKGPa3+y0jbcMN&ac=s9MRrpBG%XXy`b5t)mC20iHw__*+>2gNpjMx3sI5vc zaYhJQrj2@gP*o^r0?tS58e1UEczx~UMXpv5?zDd!J8!N_7s9|%VUgbTJ5uH=Zp%^~ z3nB@ulX?uYM{b|I(rCmm_iwRd+xt`B{s-C5DQi)#wU>M_vtSmiG0k!9a5PKq@ateA z0`$4IDs)qmAM_D9G%_s2aVW50#~!SL%y&)gKB+^W?PT4CYkWVAjm!lc`k;ExBU>iJ zl|e1d9GXSY)Q$bUg9DP9AF`srx3mbOi#8xz>guQN1hO2qw~+9+wS!-1U;Y@oHP3sx zQKnt!xX*p7rwq~X7bNp$pSdjOiKv5qy9X1G6IwPOFTQ!@k*E>QX3H^yj3e=6-Lc`< ztd}bd4$tLf`F(b}EBTAM7bPEge$$duwmZp`9S*(D1lfq*cf#`aQ|+kS>g!y<8C=$h z=8fcdnr)bK8-9GjgmSu0oAF&l$G~=Rs0ib~xYj;$1X!;)zk#( zHnwP&H){t+xLA%X>qcRyE=->FE~u;1Y#fnR3n;f#Lrg;O>zqHXr>rAeNegrB9_UZz zsCl9GgYlio#`_Blf86B-ldA3nTHe|yYG;e_izkB9rTVQ7P`|{wFO_zDl=x`MM1O8N{&S_we>&u$ z1)cvMWNIAP$MU49#Gu1z3Ow)XZY1(^N9=w8700iy?wb#TOL=I*y>f8yY#~-+t-hLw zW-e2B;83vL9mf|RMYD=b)b30Y2y4r2mM(lvZca6%?8BEP_rzV^iZmXelb%qUUHZ5E zpR@h!+X4oM2Z*J=SYrYQ@7p9~s!(bl_1xJy;bEk>GBK38(KvGcFK%ZQO((_a*qMv0 zf7Teb2%Rm&hDoiA5i_eLPjybh=&nQZjvQ%K7_Th${*G5uOOmJfc};>ga5Nwx!22#s zVT_MXH!F6MQg7j_mvXkZp^Q7OPe*_`=W8x!XGT)XJ*iWqnE=npHBy1@V^-lMQ; z)eQK$1j{eDJ)0*Z|D6d!Uvt!E$5z)v2>fkX3d6wgYp=q|_;zC#DW;dKM$)+TKL5B1!+o-%LIl2g8SC8F1^j1kN*|Pg&Tyf#i)gRj~ zEe01^6SVTZCvmwLiW@Gz`yu}eT1?os|M)|YnRbSk%s#=^tbguhNVDr z6lN?efO!`dTOREg?sQ|tJ=W%WC7(6q-o1r7FrsizRi|j7)MV{pITQTZxZ>;}En}4N zX2CJ;jQ;u((c_XhJ#Ks<*mCM}QRk(&Qxqz}sOOMSN#+ERM7~2P>8d5{?cguU1{kV~+rnshl zV?nOv1V!{tg~CXq*lrM5g#)rZhtlATD0;~87|rY<2@ax_E6}Ph;eO^~S5wKe2@il{ z-*Vr$xyZ$52!MH_$IE_a-PP)&o2*54(!$m{;|VvYrRy#m*0zNV5gCd zd`+KO*`av#z{aI%&|l9tH1XC}x$UJ|YC7ukMGPrhUk-i~c6Ic$SzN;qm*$Fiw%T9< zPEnk0z6qJoJ6Ty{+~M2k>05j`xT8J9)H$(1Rp$Y9d7R<+Vm#mD1!ZwHso32nau1#L zc+$5;`(Ov`wk6R03cpcaXQpqNz6p1zPilXqiF+yjVDrN!H6^D+<8)5JLE#0JTz7Vh z!1APng4`S`Xl9ibf5FZWKGluZizQ0c-Q}$-HKslCT<{HHDczG_*6+{J+YnYA>b~7i zs)7RehbLDtp%F4YJ@6@^Ju$UH`hYu;{b(5k3cV9YjQ_1+vhhQ?Z_2ovJs z9H(RY4|a%wA~mtQ-oi9QjIG;XfLlJZDdnfK&3YY9>+mi(ni-cu= zIB=Y%Cw5NUQ37;*tc+kXeL5rG|J_gUl?K@Lryr#Il5^Hj*^U+P6ryzJvw`5oku+8= z&=e6DCDSw@f2t+6L={=tIb$R5KhWtJ4{Vm`Kt^8|Y8VmH-f}+tZb7n>>29gYUd`I+ z*V4`CkJcefd&)BT6ZQOW4FRpX+iXh*KTR}KU^^Tq_I$}L4R;|SsI!>wbH-IjRaP!U z32h?X)InXs?7sSRL`Ja&&8pL*S|rq`9bs%cpo*)!9Al_i@{>$hxbU)Z&2+D_z%|f@ zl;e*Ra;en{P|cqpYkE^TMpl^6scwDPSKZXKC|dh`VTa^W^tAT?xE@CcvY>9A!AN{- zsGiCo*44!b2aBD7qWNvps%_eRGt(XX2*CXD`eLiVVW@QPLBt2>r0JtV(rn^mi;;61 z?&RLRurkH<97S1Hf+iZr*!jlPbyc4_Gt#)p75t8zfg@VgF)+7o%7nZgy)UkO>`sV z8sqCEwaDQPJs{MFg=ZT>+x`w_rotYwNTMK^Ym z!z5N=1GTt>gijIGy`M}!DVTmFC)8TKMZZBim&HlIFJE8I#vd%U)%EV4pp`Ss`knSO zACTkx%S9JSW({|qR!rIPYuS>(YP2glFipXduGx^q(N6Z}NKMUwf(sO+AHV+vUI?7?InVQcp2L$in$<8g3}2^&2%bNalVZpk zb>Zu3v6BnkyuZbdEC2yd>n3{b5#)y25+m=c{5Rt8{L#Fb{ygz_XAj!_$-G*yG0ba0J>hZ1qs*P zfcWT|LqJv;W>_&DF7^<|5oPnwB?IV&&HR8$*TIO0vN?mdS#FMn(KDklR^O>+Ui8%L z7o3^H|M!D?2};wQf3a&xz@wSklK1W|gfVW7T+RS1p<#WLrY5kXCg2v$Oj-x8a zt6ru-23VwK32L4^TIGj4@vSN;nr1ld(DEVk30=6PPf>4y(9G`2J@7+`jH7w?8{9K9 zXui@Z3ho;F3OG%o@I_DSHn;d6c(Q;A^8`UXSZ^34?csDQ6QeqFRXCi?39>?71+wTD z2Cn{UtNPY6cw5gvV)Sj90#warukdMhjcmyK#Ni39@zW~j>L%6&!=EmdoiAo8n={f_ z(98j6sT;14K=%uVmse$l8}IBM*($(#SV7kh7U5fJh(T#$0@B-E1W-_3@6HbNCS`E@ zjH6TKB9sN*wYB;iKxKP*Qd8});^C(iWa9|`#Z8f)$5 z!C5Z>IEar_dt%_lTL-}w235IALV1-Wv6tq&)uMw`PL=yD(rS$SQ8psvVq5W?MZkXz zxU4pvgF6Q?Pu;$d+W8P;?eAlam=O6Ek$crC+)5lgo7AQt1{isIKGGIqTJ0LH&D3h% z>U+Jqzpb1Jl+{ce;25QUl-y1|$D5++vV|6e*rq)7YQB*lGKX#@*32K!J(QLxsqH7C zWvjzuhJllGEal|3nzczg$_v+u)0>!STY}!PTShHhs2WzWza3&bJXyr|d!K`1KS(C* zG&R>;r&*rg^pqRmsdB|rd&OlL?|z-|1GELS56B8Acyq6_DSEERZi9tDScuQqg_29- zi*6L|ZBC6_0Q1(obXjsH#;=>S{?;f=m$@7TfJrFU7>umh*)|rntTnOzFkXsvbkBjV zpkryGnB8bQ-UW=Pd=|?l>*4bZGN->|pTvA_d}R~prXtxG;rhK|dYR>x+|R0Nz%CTkJ;^nBMXL#ns7wBoP>6Rxre62lj)QU`v9W?|tDKCaj zRp{_t?LWT{!AD!sed?5HQrjGeu;@F~Xt#C{(qayhYHSFyBGZixb8+_~UA$Hk5AD8y!=!!djR|*S{N#eJ(WClZ_026KCco+Lj7+59e0q^QMD?2 zg01t2ArpTxr_8zfYy$SDd;4BZi$h{^$Z|n%fdIoF{loPqrX}d{0;#iS*>dxaBa9E{LM}LxtOEkBB%bbPrKO14 zOZM0Kep+pyQinb}_v>7IN5^EwTtz%0@_F5th&%JeS!#FCzK(1vp6am*wks^gs3n)^ z#5itRGeL_c6-+%xz9a11#Wdgd=g2s+ee<{~IEHLDMToED#jg-UhzVHHR0K=&=sTHH zwFbT4Z79TGeW9JRMn-8?uj#l#EE}7CAKNtEa?`7ZxLr9aY6mwptpc*+)AmjM%9dui zk9f5+cGc-kUkO^(GuX7WMWf5%+57L9N?f@D@f2qbLAi~L;RD3h{s-ce(V3X@ztCq8 z`e|gc={`ZW6D@m_FTG&>`2rZU1YtDsq+yDF@zlV$5)eReA>~|sA~cGc8I^FQ$OxY8oKxS%8vbzR8L;VgzUL@sr3#h&8q}#n3^VgPmZHFDjz>Gu^|R z;ml1~L26;DX*s%faS_=kS{Fi+Pae?K>(( z5klE+)9JM2 z?u%Zk`dr*BA&P(g-HfSceh+5o~{i$ zA5o!&7juG6)XiSglx^MXNqn6wd+XcUr|WkVF+mW*t$91zB3a}<*V$%{28BVaua zH#D&0;Ea}VJ{UoJ>mfVb<<4D{6j^>h9z+ImZhl)L_UPmVEoFiWj0ER)Z!}?P{TFbC zt_BEu)cAXq2)nKU6G~#IqO;ryghqHhJXvV0l_Gn$!#SMt6?iyvg2|t^pI`jg`FA=c z{9oOb&E@tv z3frulDX2pt*0f>nTcGi0a_5Xqfh86V-^(N$Cv8w^t(}4#<8JZhb2${OaTtSoVmn^X z!`PUs@^-RT0f*du1#XS9v=wamLUHqquv=Q9$Yfr3$XGVDhSlopzqEB>Cz^>XoFxcD zzai#?hCY(^uwQ!q=VqTTT3z&sA_*wooPD9}urib1KYom~#|Z!2gBiA!J2 z8#>@0Z$kf@Y(J3tlOBSp+CQsh|C5sQBg*{am@>Gi&RJTL!o}-AFqZu&JBvnVhb+>) zQC8Kv_LSv2lJ|Z#AS3y<30?MWX~7WB{hGWKPA783{so<+(-OF!5M*x3k1o7ZtIT;$ zTI9^cAGN^R{P!ttgGycaQ-FTXsO<`p1-{z{Fcjn(*^V8)ub$=ImuixfKZMq6Q-Eu7e%u{CI zU+%51R~^}Hd>g-V;E}|FT>5=17-T4S!CG2D`7?9^;(PkY6)fx?!SL?I#Mg(vGQv3; z^9t}c9NC%ETU#j z8`p75p8#n8nU6HhXOmj(WPru3> zo{AU+&-|QwUmf$B%oh3)bi#V0!-)6CdY><*2mNZ>|Hl0KDW32!6|XmCKUK6Uw!DEH z)+-ewuDbtKv)ggrR+yP@)DbWPE&Kk|mXI?%n!pQG{0MYm z{R%PAH;_>y(ZjK8&wDK98nYh@g(?&z=;O&vJh_y$^+4XT^FK7ri?SOF7=$C!Bwq*B z*kN+I_-0MseT9Lnp7dDbRkcjn%WM7v=#w*7mcQHlxi0>8<3lApPP=JfH0AC=Ctc5y?XZwkqwu_iplZ9y zgLOK?%$LljI?yXTQwwyX85Z}cq8jnfM9Ap<$CLiM6?uX@uL9SMH%el4F@SWw|B`Bvuzu#C_4)L{@KkLfAgZ&o6oH za-oj%B`4p5pPN@d$3EKUr`oQx5wY;UT;0Bw8mIlC9R000z%72|fUgR1|X`x7qztU4}X*2fS!&0X%A$1`ueBnnOlB;JJPYPzI7Ct=Jfm61$}p+SHU6? zOVNdErWd--qT2`Xlg-vB_X_~`Grm;OhHoT^5Q^1m*Z>Z>Iiq7FRw9ON&6zG!%=4~P zL15z-EDZo^+6$B6ged3tH!Ve9zrqf271=d>lUAH#J{bjU!cR3FRG<3#g9Y9_W25Mz zL#dgWS9|2~ZClUiRqOd|xj9o`Np2pZDJOA83UihX8Fd^i`rft@;FYT8)~22RM?Xz5 zyjIV4LbWsS*_>s_WQlnLi}=I%(Sy}0)B8X1$uZgt?>NL=v68+HRMqVR?cNep1Z<^m z>%{jp0qKzoh4r@-e@&~em2rU4DeDW*OtP4LKZ|sR-F4ZA50}%P8=tM#&fUH#5q-u1 zRz&)gT;&-W)q@CR1jTIh3CPJfHNk!#`{rB2Z0mvsG6gIm>``rb@L?X2f_Yyns;O~& zyZko!-#>`?s!gap>n-n+%bpgpPkR*PN{jn&8aGn3CWt_HFY`AP<=OvQP zfHY+Lu9VN}5@kRPe|?Da zMrt~`dYXGAsl-D-j7s=5gS%I(g&eElv)q!|JCCQYO}YNrmW{Mhu4KO=FYEEvgZiSMRw7Io#+{VI z6IEmR9`E@DPbtKrIfP+jswK2*9xsG#e|edYVssV6Y%!yVxT6Xbj^RF76z8Zf2inKK zHkAFGrk>+++Y_GKwk1d+xoX}GD0f~h@A@zt)COO5V=g9*3x=CIyI$jux%<8`Ysu;V zJiQ4sqMv05B}is=tR!hzz88S{=RG#Pbbusba!2aKpl+qgXk}*z&LCM(*QW9M(AGq@ zT2cKpwsdN&%3D;{kX#Y=CqiSmsoo2ELv;a7*Y+R3^+0ZD^Ab;aNnPjbOlpX12KRq1 z98m1N5@nQ|Y{IU(aA|HNH-$_elzaL|)0m+h_~NiU)z&1&n}so)Y##>kHn(zHN3{^OfOjWR%t?D+legX@w3R;qlTv&J=0D#?yT_-hn>qWm+q?sDN@wb;-;zZ!$aZ4FFe_1;DvQrIw+65 zKChm$09U4LI@!)`n#mbJHH>F86j2_+TJ^9)EQkoOyeH+03?Cj?a{iVjo4f1lyro&b zrm!`r1zW|}b-pb3wQ=QthwZMpJQ9l;mAuh|M1{QIyQ{j!9r-^GF>klDGLtLIa(o6b zOaP*lca88f*XCkH^zo~UeyF}+3^QuMbw?jmphxtto>TrH-v;|nsLdU8HW2)sQR1W~ zX|Qk2RvM45E!w%l{v^~o4C#^G0(Ha;|2}rg5AQ8|e*MrTMzw*!*OqE8E{-H<`|&rH zP8nS4!XuJv=YP4VnU444^%6>H_aD-F2zYa(_xLQU$55T^{K*>QYHgIjOX4O)Z|WDR;LJG!~2ARvRH;UjQoff zO`H_ot;lX*u#rM^j|B^j69^x47f-Mymq0U%m0SzS2x{yNM8`%5V)sU87F}HQ^uC7a zqsE}U0~W==fypNxo+F0|&= zDF0~`eG1U`x^n6#{QgvLx3KxA8DHI4O42FZyDnvJSJ6V}f9zW|#)}vsxiO}~hLUHG z#kIn7NO8b6J(-FnvfW0Ysp{{}CG8h`Qlz7rBQzj|d!`9Vg&*$}75!a{a9yzeD`x8C z^0L5(_8TQG5x1&OkCoo!n(O6qPW8Eo#~U{^bWG7k>)XyUFeX>4zQ%}gjhJCQ9N2vu z-c(b^RC`&|b{WZQWofSEpkbU^%j!AYFOtb9S)D37RL_%=m+_@?`9XW{j>z;(9T{Nt z_#ZdRD}#bn)>>61v29%uaOv*bB#(EX*X)e0NLqLP(>>|Guq*M!hJIN?F+Nh9VRvIa zf3Lu>TZkFKzIKpJs|BA$gDF$iUz&Bz$|R?!H)8cU#xspZi3JjzlNFc4vh0+UY2nc- zuNd*k2s@70x~7S+yD<0ibZrozLQTHaLS#ojCTR@|@0U3!#*}hZ7KP4nV;C-O*BMb4kN<3|gJ(g_+KS3*pu=>Jkd}|ne?`?thTW;b-xB5{ zJ~X3IkcR3{T*QTb3Z7b>e}2G)`Byqm&EUxd&?Hv9Byagc>SoQ;%lgzTT?Oz>P|E<< znC$^uOaeA1-u^N3N$L}t!$&xHB0~9Wq58b4e$UHZPxV09c=P=7NGJp4-7wLxt7(k=-WNaf}+hRKOU`ovBwHy^aMcZ$1(MEpH)SC8Y^`3M6)FeT& zae3XMDJ_QOI(;(h^O|D->5+?o^0_G z$Lje|GAwW@K9%<;i6_7z+G!1KqF}N>N&nX_hrt(#ucN*WoE7KHY!mx1Z*$}O;lfI< z^>4N`dPX-4%MKA$joPEG5p6hk5suk>I^~T@Pf)_1MbVP$=!NlY^gJKTuVmk06wu~7 z-@JNcUFjmYGt{%pKWBd|oc70^u9_Y6fpB%$NKeMrU1y62pw<8mCsW#)Q$=^iwPu#4 z)I^lyGd;ye)j^tho)kWM7%^TFIhI$#hW?O!&E3Xyp3l7M_!JZ zn;EQ|{h}5|dwt>K@m!+?Ym!A@9i%li=jngskL0d~pT@k>&o#Ext^1LW1A!@9p&)Dn zQ;9CU&{tre-qVvc&dGz%qmb>?+L=B0EAFW>wuaR!>d@TXI_&~O$OI$%bufSXSPcL> zKDc5pWi`RVlOVGh2GIBg#K=1AZs3Dw6=iKF%o8@VM4$cq|0Go#C^MY1@H4cQ0Cw|N zzeLx(zRKI`fSz_exLlvtk@k%}-Y2OPHx)hibmeKYNqZgUCPCj}0i6g+g2Izr?-<&r z+yIRgaS!TM`KR%lw%r!eLF8N1zG z8S_v;vwj6Xf<4&MdTPaFIWJ?1Dn_G95hYp~&+D`|`IPi6l>lAuEyT>AGp;6>D$;zh zwXgjR7azf5jbsH2=0IG;@}Fh40peZQvu!e_Sa@|U6tvCTt(bC6ZB0(B&wlMXmo;r>F3+^PmG3Uz9)=|{Nx6};S?kW)1cf$W zNmIJKzj4HNMNNjbjV4#`M-={otEx?Xp+}B0HnKl~;r_4zB{+~5%wB=cL&jeQH#Wq- z+yO)?ROWw7T&!}UPIv{r$N0xf;?D&G{E~$7q+*W1IEGBVf8sUVcRYAQN;<&GL2&ei zc`>=(Bl%;IP8J;3 z3v$Vh)jw6kHax{awqU@*3uft-HDA_(nyh|Z$?IZh z*dleSLVj=}a6$k)r5Y_6-UfEgf$e^-Q`bI7HiRs44Hi*I3#MR4#}U~=G()*PFu94e zmg0W9hh$boT6TA-q;3v`;?L8?a(1qI@pkM$UvA!`Ur}wVYKU9YtA5YvsK_wGg&KSA zwLe&>F#DCPacOkaf6N?yvhP~cj^!j*N{8-Aruz$Tld7rDKsFt(muoLjBAL2Ij0NPiXF%cLr62Z4-4z} ztmA?xO4-sIX+f;ai%rGs1m6Y0sNv63>bpPfDN5*Tty%meh**cX~W4R|ZZ>I-c4O#iEkbqoym%{2@gty#rnkD(*J) zWc=8yJ8QKF@}}&-o*zInnfs|hKca<471Z=`>Jt$kx`|U7XFM^oM@65=^G6c$2hQVE ze)i2NM_X~890+*#c29|5VrqE(}XMxC`I zGe%{zf?7JkzZ^8AzV{hZH#h@be-8+~;u;7?PuH3A|4aL|xQt=^^`cbBw$QN=`j3u3 z-#KK>fR-^nY`5CM2$e;D2jds^zIM~Qo0QpK*ZZ<62ffyHa%7nyRZ{Wbn63S)+>h?M zb((CeD(k65)DxqKy4F@vSXEz8Le<@~#@Po=4rv&<;5!(9`z3|oRFQ(~G$%Bn0+OA5 zfi%XK?xi`>z86Qjy6{39q46ev;qSq=#q z=Y$*eJ!(utf(pkADb?ah;Z*?_xyzD^0Qkw($hdNr)K6Syr|m@6BSKl6mb9L6b6<;h zhF&siShl}$-<;I~2?ZAsAH_V~(cz95C6+kH+xIUfcBML=ZT_^^h0E>vK3sRRJiV@d zNXmDdq8BTbWVmy++MV9jno9pNLNYRlP8drvm}oDOBH=H!TW`ga?Akqmx9*b z-0t{kcK7&WCJE+^Do;Glaiegyox3qt$A#jY7jf6> zYVK{fJg!`bh^GSt_xdwn5ha(1c6DirDDT^5Y0j!>@H|~VBw!Ir*Xd0U*QIOhe)u^n z^6Do8ccvf(QG7(G5UV>dqxXiu3WIft|yxWjtRfn5JAk^x;X zFAfXTMDmYnZa%`{X!{xYlpA{|t<@(Dfubyf(I{Qo03|lUG`YruQJY!@_VseS20Ga9 zL~a$i{4~^^RwEwyp>XUVJ3}j|MEMXjM`lND!B75ktx|8~#{lx&ZN1PU#LNl{EsF7@ zd%jc39ZDuU5>adT#*;+>-VuKL%*~9zgM!T2*x)HxLsrd=P<@fb>A3a23`$0UL>Tf* z)$*2@s~KQC>og{cofDw2-MAU!H8-i>u|^|mfnf$`p`T{ik~3<+sC2SqX9YWif#gK>Tx>;A#%BFZb{(WI zc0OSowZxkZFtrfGIsBQ~Wj3<#_NlQ!@Z?Csz-E_c2D*_# zjee*3<)75Fyn~q=hO(>XqaSARKSE|2`7zPj1B%N|70`b>^T^WU(iA)R&Mw0$E%x`Z z2S76SQIHk<*e^k)+~7SLrNP;6dapVYoXv77bRp*m{K%Kcaov#fa?4a^n?6e2bb5e~ zg;a@Ec;qdQdV*`5^g66^A%6p%FVPrD-{A|B3KhuD^wtOqXCbBG$xmJs#~pK`SLLvm z7xF3T_p$K{zmFmKpwBZhpV0!USav`F8Ow$0pyrm0xp7_}?)R|*euD82QNkpQ%yYyn zf%8)Bwx^soH~TtPDFZ>jtWwiE12k5;qHSEuE&Gw2uiotoCA{EP! zgGeI0lnY=Qz10w&vigSERKLuq5xBbQ!0hDj9Ga;lqpLQPyM>2S|5qGJB*USd}c z?XEUOhy^r9s!ydGMyF_w=FT$kZ}!kq(7${QWv5D~F+X2?dUnpih~%;A?usxtdbLXq zeYG`UsF)ayzd5yAw2j_(T3>gMqlz%x|D^-Rd-gBts#RXAKxU6*9IarAdKkO3EtOww zGUt^HOQiZ=wQasYp5QkKX`EvV&F__>D}U9|*je-X!|SVg>U^i{?BV~cnC=^}d0Am+ zmuJU4RTFT5$;8fq1vO={ubuz?KGu3rYPpbqOP0x!0!{Z{c=ecH^n3qjC8$dAXAVOn z0r?}Zx&&=HvuLzCt_N<%+aAU1+s(u7s_*HUsRw^wvA+3K3OCTUyy{-2u|QV}J;%vA zdS4wK)PHeX+0^fOuyokDv#W2O-cW#E=27dze7_%9^?bn}L&!;^h{-<)A+iRn*@2r8 z|GmuEEEQpoa~xK5g%V08c}i=B8WS3Dt)!ZN8t)PA;FnS#xE3^H8tGh-`7JS*lAobT z@Q}-SL;d0>I^(LLvgfs8QGEs3Wh5$obpHe0zH=?{Lxg!)O3F%^sf5)9R@y<)qV+jE zSIxjSg6ib9N?0>fy5x~tRq1p9|FH0#?^Cc=QjlKx$NRwV2wMudYIhgyLv04cwrO-h z#+pWKfkOS1M>90ZdEF-9PtGN8&aj*M)7YxBRsp1yj%<&4aDB41d<$HYjJECo%lxmE z5~B8rzb8L+V(_*p@3?K&OnXV`snyn9;Z)bsPOJANk`*1PjW3qV-$!f4w-Hcz6 z{dwhGteV;jv?B@)@Kl}}xUxBu>8?rQLnxgpl;jq0x;`W=@U=BIwK+uN+(5Z=U&J4= z7HdE6P@$3qSkfYXy>aLqHv6(dq7lcI{7-P2+t zGCyyZvcB!E679hJrkd7&k+XcL_cFo&&)6*I-C%rt>YvRs+}$&hAKu-=cx}h{rP-MI zJ3&u(by1`RE0oU2z<-Rt1;$)&$ULTJG~?nl&Pv0#b~oci!j&GgK?@pe%NS`$;Hr-J z`Kst=^?r#Ve3mbY=(IQ<%M00o8uH^H_h#M)T}H^$Hn4Yppe4cK39=RhKVDxI|E<u(vIkncCj3t&+Iv0>dEqcOzFW5} z$+_pyB;T)pELxrJVjRIzIOq&QBf-0)&c>25NpR$g{cqqo#h;E8>t8B%KT>khBJD~Q zkn_L^+XH&xqWZlsxcLV{b@6nQV(_ZfS&L5-;rvY5L&8to&CwGLs&0Ns%l>?|Gc57P zrkJF4#wKCwt^OZWiPq4EIOG(33n z&7E{EniRGiDkFXDfMzl<2{n63mlRU0+qK(ath5fv))Ls<>(=I~sZ5>H{&!Vl)nB5Q zi_4~*rwO5SL3U zv`iiqb*WNuk*E+S&Bd>YBT@v0UQ6!&J7%?Cl6k`{`?tUlj&A+P6JS8;}fw5ZLvs~ z%HN9R0oDxvj(1_C?pwH{9>rT!AOCrsL$0bM8p=5#GBe%^1mQhcdKr2hhxr=Uq*y&m+Z^j{i`h0c1$Ks;Svr=-i<^RL9y z15(CDrW3G6=hJFxMHh@OkuGA>LTe&8I-?bYrQu?2p<7GKGp zL8BG>`9(K>+FapK@f$VaDIFMy>;){aq3K+f#Wt#~i9d`$skmg|mPZcN>Z`uPBHtgl z3^AIO6gJbTu6K2`I2i>4EFq=UU|PSA#Wh`g{P0_JfoJ#+S|;)f@xQfP zY;Tfkz+&xvulCmqS$S3LJeP(^`l4=fUDy6=(fg{mz%%d5Ct+eH$|1s8#Fih#i}hwN zdSo?N6@(^FxOkNgPtPrdcAD!(4w&bZddY4~$+1-yV|Sjzh2}QL1-6M_qt4b^O+=!7 zFMFOI$h5qS$=VJ|EUCJ;-!!(O;=0aNV74QKgj|CwLHuWdM+BT`%oXb{`daWj8RWy~ zj$B^s<=WVIYccKN%%YcUxrMj8v_w!Akst2(+=<(^ZUe!Y8ehVAVoXo`Tg1(BN@vjv za&>V-PEqD%4ZVk}j)r%Ra=9Mxg;SDVDe(FJ-2#HKU~7&yBYx4iW-qfhG;DK!AXo-A zi9oAGPBXCcrzGzF3ZGg3_I|Szk4c+kd(oVjthe6Ty}EHF<#T|ti~zO0sfS;SmS#s0 zO#Tf?(S(?b-F(V2V7{W1EJ$=$OJ!iAO&PND$P~n_MN8YQzTAjcV?2X?V2&$}Sb^|m zBrsXs9q=BYHe)zK%jg$fn|c}a$t3f6^#ad34P0}ns^8*gjKhGxRDcp(s%fT8LPp2- zJORAlhWuW(fJx7ps&)!s=PL7k4T-i;uH5NL% zmQe1?y!~PX&)2%VS~rLy9cQ|z-HbWL z&IhsHfvY@}j;_Xlm$K|o<+ZCu&1)&1appsXoN*oO2Y*rz4EEh5U6b#5iiIrhBbmB5 zjk12dTP`&|zWgRr#9e5}@!^P4KT31uc00he^St zQJfBPV5$E1F|v7@)`yu+D94&};C%n;=#!7puW_F=$Ns)JS;-lS>}wB8>%Hsb(7(9N zPnLSs%QyTr@N%1IqZLX8$4}G6Vt&o-S-}hk(+Vv4Yt$^wxl?vjG2LF$1bXL7pVN%> z@{2GT>4wgXk!IE8_^@Fr4!afK6ls~CxzS{!@-=ThGk#>IkYxG#R&j1eZlu2|P-r1Abh^?|>6OmS~jmX?);7hKo)OS4EziioZWotUp4e>h!MR;d`V zEU36>2Yd#N6zt57Fv$4NjJQ>+Ea2>}CMwkCDpPipUk#aBlgxf=ytLOfx2=i%b;T}S zTFT#+TVJulf`JM1AG<@7Z>%m)yc&DaddF&GFisLH-I7*i{CpzYM?Ad^NjzD>n=LVI zOep2gLA8QTrm;a9Ke>pg@QcHb;rm&Xs_${`dB$K`;vLVF zZk;Hj7W5_D#&DG4cJ4SE2#YdHb#^&rBNVtu;_q*Yw2_xV1?v5F|in2&uIGe$3vd4LkMT4xb=J2u9z| z<>;-O%1AmL3E5!u4I>+;)WSHD1dE*LU(1JmQoqo1W1H5(*P2e3lY4FU6rgb#w#?&! zh`$|>?L^U3#n#cWm*OM0fz7KH-^UNKsqyqr+%ngITGL?KHGi^I6Y~!+ZcFYocF;6r z!EygUSIZnv-KnTiO4yX>StU&HNe2U&u>h*YmJsaJzQih!ckN{%h%@X^i?B~eq?xuI zRTJ|M3RHo!(xnAeyt&cV+^WSBa~_sAkW{a-jV12-mJ+GvD35!d3Kubhv@PAQZwjVe zaB{m>D=*O6Ubo1sCWqxqVJHVEoedrUh_EgyTtU?>qZ_OvleXkYhVRkyJZ}sYn)Twp z;nWY)?hjOHXEAffM8^{RJK<`oMXy{k+U8b|YW_Y0y3rPLIlVTpfb&APn&AeG+ z!85wZZDx%8cmeV`9f2&5uB1|Lr+%o}L3C7&g($RP8?wUbKl9)DUl?GaT;T$1ZSMyc zoa0DFae(Cyog&i`^*s4DPk;WcnrGkt8UpPnF}2HVtDhAX2KYuSSX!_GeMIhxqHlO{ zy6fD3Pb8wBAL~ zxX1hTjXCBrRkX8$JnQa(s;c(^cRyNw~$i?>knz4JIjaT zBQk^9j-C+-Y1~^=Kj#EDOTUh`g^}_?^mt+^@7fT9A!qoDNF;q-=ieUwV`e<+EFd<9 zUbv&wy>J-ms#bt97vG)+CAA;_VPM)FBZZy9$m6_|bq-tJ*OFFg^VByrU)%h{kOSmN z96x6zq>MY%s;(i1^jsz?Jjzq6uCcZC8rJL|@G1U>WIZz5jC1X9C{nj*Hn{~7V zYQ|2A)gMxFRf6G~t$6l>T|@kA7LCGxyqd76K%A*@w(D|e9hH{O+k3}snwDnE|-*&1iaT?%lr~qq%4KzK! zer|H#Z_lg&8mCivgGc=Kg@?R}QQc|xfMq&-zWE@d_eB#edlM;kGp~DXmPetDQyjq^ zzFJb($%>OS-cgP1wB!(l(e$qDr~ypm$STB58(l#9cm+~1KgP8p+V2S5;7v4V-eP5) ztSIVaa1+Ub{2h1m%N#L6>fS^{u}$UCR^~Gmq}_xTd3=YTI$+?>O@$6TdS?Jn*b*N5-$XL{C(f- zV-x>QL${q8jW7H=3ul{U#HT%1^1E@3M~Xvh6oqI>A2cswkQM8z*sQ;$YPQ2a&;kP2 z?JX_=&7__oBT&vgKZF~79zYyB;gO9wDXp;LlvxJ1rtxT5Wz-3#EjZlJBaDm+5?g3$ z%)bi|#7LY+aV?zHB>7`UXruiLVCyH@7KxT;aBrZg7QEtTF;ka2GP!rZ0+x1P%kt<)cdZd9X)-xx-dwCiss-PXV+O@ErmUVU zC`gJee28$YuFaOTP2)N6akrVWx@SDTq& z!+w&zx8|v;;;W;?YLFoJRnxXbKLdXsD}C`S=Lo$JN^*B;1p4EDZ9oX=MBK=v85Gc3 z@%va-k(hdMYwlPA+*6@D%xB!3wmyY@51M=aIbfM!z2zypf&A;y_vmN`s2@A zqfJa~54ofLlU0(5q?EBGn3Wx?dEPfyu(K?rWs@$5)>lm_L3yjf+=e%etJU;$0$1X= zI_ep>h84?#qMDzmwuL6HY3Fx9u&tRclARqYS>Xw0lm?%vAYt^JR|VlacEoQa+hpliQ~!8vIsy62_uwYu9P(nf$NL55-pY z?ys!->N>YTAH#JX^G2Bu9$H;%y)UI5`TqmhUHg-!3IprmdYX9m78}zj|?PIDpukKuZ5tX>C-mcr)gsLPz{FKzo1LIh+ z(D1Q06<^5$Iwp-?sY8l!7J92s(=|~?1SzOGhZPj@?+MR?*C1TXH>#ku&>A?jLR3n4 zo+QzP&JM>&&3MST1-KVHySlNil}h;B;BcjEZdXXiUHZOtqU+im>;UTdJi0|H3?#zf z{ThYr{|c=(-6T4!h-KvNUiG&hHw*~KOEi3cgxahLfm!$F3etPxN!c&D@DjX>Pu;D} zengop=Huz0F8rSeM}!Cy+7sE(kmTg44Z1Wimw}oi{(4U|6FM9?duA~J6NqwZ^)=o= z#Hi=SdeQ6>XADJs57VCx%Z2)k2?hmlMRi??c_(&;&~xb>FBO5y*;~MM5k051pzTu3 z#fW-@-dw)#Btl1xkK&xk@GYzIUw^!||DbDB?~cq54ncq0&N_Ba2#uq);UozomYA^nzC#G0^Ygr;MFi! zeR>YNrJMb_y0-XEvWgB1s@5q<(m`_8>X%Y?B#*odQs1|m70!jb+ryrX+yd7fagp6d zq}t&g8O3;TgK0t!vaz(IpEd7*n$zr4+Pd}*=J|9Ql)gs$1KuuHZV^`G5{aU66yHa8zOO>wVN+tKK#--)Rgb0~8SPjOMI2GNtRF0wtjiG9ui=y(UtJY1a zjN?C)78z-IApq=PEU+ zOw=U3uP7hzC`geuZYvOBB>mW|VBT_uUkV@0oqrz9?57RzhEfLIfxd@+lF`M$eWt37 ziT2y+XC*jn3IUL!WuI_zLb6uWe%S3Ec<0u)K%foy>QG@N1l%a;+BW`z^UR%il$*sn zG%%!;WOmm?XyeM=*^ACl0i7HE_!wiO0OTyIrtm#ZGtB?%5K#kQ^tRwi;`RLh8f=HR zcTvdx4w0T12^`}nu}@eiuL^mgLd#l+?9){7=NYUBhZfG~V`pj{ZUnWd#dXX)zg9a_ z-4SG)LYUnK1tBAAA(C$*VR_>>NaZSnwSYmcaZNEgbVhzIf;02DY$WeF&G};PuK@&3 zJl#wTPbJ|a`K;E+OfBMmZCmGRZTkl&wAyDoo&r1RpVY3_A!l6UjyS4~Pa8%JG5>oN|U6GoUL&q6bl(l4w80OG$SSY;_+8Mw#_r(I@)IsB#cQ zCKj3rA%(3%zNaiPIMci3F0EHZ$UqU+ps!qajXEpR!*tCNnK963AzEn8{PSf_0a4EP zb^44TjVP!A#*1GGrIZ%FjJGP2gd1zIUnHW6l)A~E<7YSPwt615$UGdE3D=LI98Lu` zXbSQla;h}hTW8|y%*b=9z^r5%5-Skh_^-5sp|t^|r4|rTm%Jqmc@{Khh6REUk(uA1 z&a_qGtA*J*(Af=M_}96iFLO1+uP#4nj4f}V&aD*p{Lno`@s9EE5UA)0+mMp5(R}nYnQEdC=P+R%;^BUzV z68R(5d_xagPg%18Jy&AG>0lYlkCB*lvdU#gY)*5r=~&g>lrQxUEAE&81$PoTxZ}$) zy?VX4%{d5A?gecbQ)4h9`Z8Lr^>G@6)Oh02HwS1S$q=4-VUX>rB#L`Ox-ty;RxhJ{ z!GqYqB+Cp>ua{ogP1Lvoj8-8emLFq$^J7n-Bm;Wzgki z@eHEd^p-erk1ymgA(bcDoPj(FIg?mtOp4H-5^Z(Z6fnrg4iJ${+JuRl+}{0H%YRQZ zT?xA{d*{w24M6lfkYB-26-Rr|U>D+b6`_sy~js7guE-pmt-)gK1puv3j8u(Fwf`$;Y=-IAP zUx9lvjs}C>^%ly*UquC`6DdcgY_-jw$Bt1KCRmGfQ9$2=vcDENWIj%b zDm>bSMy~cVccy#pb&2PyFh!M93co&$%X*$1AlOUQV-~)|#wp>x*ATEZ16)oNur)wx z6X>9)|HKNo&=Et^>=MRTe@y2_R|EImES{WYukU`QO^qO2YdpsQ9!z3YG~5QHZ%}kw zjfVx9%2xH)P1Sq$*lCEVL&Tj!yRK8*AqU8&JOcM$+jzE0(+Dy{d4qqqX8hve+^pYhDPdm3oj>xosYuPOpjEm_kAxEE#)?;bkdTuQG z>7c{FY?Mdhv)3G_gZu&c1`$uuDkbRZL&Nb?M{5QSHIt-p|M>hzt{#1CKXe~|sSjnTori4#PsmFlXB18SNlHlht zfi5_EM2X{m7E`D@f7NqSM%YN3ydnPwHum#sTS{;HLa{zskER+Yf+joqKNnNK{>x{a zunE7?Tjo>Dlr?oCH)9eh4yi1aI7$_s)A6Z1Hu82-3_jFX5{ztZB~o}J{M@E!E2Fz- zD%qux@v6N|L4_{J$Ub~{xc5ff$PThg?Zad6Buar)Z`NRafFLb?CQpeQ{h$ucXH8j z@%*5E^_kzdF>9f;cwcyL_{{{=0O`~t(1tK~P)O{nC??T3$wv2iI&OB+uU(1yizUa=4$Kw+%MoW-ZdTEc4VHO{?H zuc|Br_@T4_>hr$OBwj0O&mY=mU;UW!bf>?)^5eNbQxq` z400;* zho6G!pfBa@nkRdDnk6a*a_UOxA*<(2nwPp_^f!Z7k>;3>E599>oiN zY}=1QQ}Bxz?@e$hOq^01e08qRxqGUJJR#L^3WKHr)0u8sl`F!naoVTIAy0aLD$r&S zY10Cx$zrB}GtDc>lK#YHgF{R|X+I@;-W+J5gUFXm!>WwfXy>nTE(dEGJ@(Z^Ukegg zT&wS6dF}@>%d}+jp2{lFdM(bmSCN{)6n0&ca4N`FCHGXJrFVK~6S?MUma{aL{$_p0 z_K7#urEB)aES&fr!(=?b+-I8V0P#d0SKVoKvlK7>^>Oh1Ym6bJL6Bh^7lU zl{sJ0N|0`f-(%sM&EEi96HUr zU_0JL!Q2@cc~^7gGYFHBkZU&bDL(Zt1F#Do8AErBi*?54P8#L4RWVu)D_ojx|NHY; zhF4)UNSZ`a{@1^O4~XKXs;(Z--=M1bLGZ`#J8boEnq3J?kX93a_0B0r>O@iA_Hl}u zm;TJfB#y{zZJQ^Zv6^;!06lZsSn9BgT|p>dY2&Z_PT&xDgT=+%_bbZGf{Z;q9Oxp8kFH6xPqS0FDGw>$7XRwCUE}>j$Kj zW$KiJZ_JfcmJim%Nkfhw7F%yod81hR{RekUe2m@p@3BQnnO%VZGUkRpT5W2|iiR4V zuqOR0zM*#KY1yL!Zv2P;jm{bHI=rNi?#d`6{KZ=c1_k`T%2W(5Vq>YB_BoweW1?46 zX2qoS-#i0vN_-(xb6SE?9+l3{p!E{m=;s!zdEb$iZ-jb+dj`bewOP%s`>r~rTc>tn zEo*BtK6g~SJ5AGO<=M@EcHj;t)s>bq>TnZfJSSygz1dacbFblgoF#7YEvNrH4b=!A zy_NqXzHlX+`bzIy{dp_}LSjswE^A4KQ;=C133P-UNg zi$j)vjK!*vX=#1-^S(yvIt5BPzpyKm$F#-4xBmxd?eO`ALPcZr8n!zsvNNj zcI{lR)30|% z*Y4@4s!ZJs--I4~97>5BIlQ;`B&WXE{87oRAc?JswQy&{u?l0t-3A>@gA#&-M|SqW2kpdlRys9~2iAslB`SX3iU$ocpXzfqY#?+3AwWCFDQ@ z>AcGI>XplZ_HWZ)e)azBuVN5B1* z-tuilg+5<@Vlg5mBX)y!c$}S`F6)s*?D1Q1e%*S@v8ms}(NI}CSKdr3A)7ckX`z#$ z1#)10=W_(R63gJAf=AM6cNEN9nqJt24LtU@HBkvdDKC`u@Xq;M{!U+ts4a6uRXs(5b>5+fm7)IL+X->mi)mLrUD7GJ&e5#2G`Fdglew0aD zp~d;w9pGEvwAlMCboN#dK}86W{PQ7%Z93M{Z5A5mzGPx%= z(w}^lgpKAbiR~KPBF35pZ=||x^MyU;8+x+oIh>PkUxc|~JQpnc;?4gzKb6|;2gpra z$!Zgkez3*)MuC2O<6hg|d3^={fAzY*9*O&uI>CcTj>loK(U)@(M!gO{kDce)I~Nzh z!-BYYrRwvzHvOjo@zlV?#-3HwvaX`}YctX-`jq+u356UAljxONZo{#4uN^}IRR^}) zHMs%>lt)@Du&4Cc2gDam2H!pFE)(GfSL6=iSZjQ}`#RGksr+2c6;3Ihi=U0RH5xs? z_Jo?;jk0R}dCX^wUoyt^o(X;L=K$)$%)_F8>yzc{v;#}Va><=|wWS74H+gk>h)m7! z{!Q{po5#3D4RmSgXf5^BGKYQmuIeN4V}EbF3^3e-86`*LKIlh;_6@&>@RrNPcySZ& z)7rI8RFIybP#tKAskD6&50QRaeHBIBB0kOefZG}vRxi|*EB$A3qRKq;Daq z{xg!LL`2zDg;V>br5p5*QJ0uHQv+E?=6~Yp zw2ara!5fA4`mvA^Ex5iX<<&2dJ9Bmi*tBNM7CdX~eb_%S^9EP!w@F$?A6*TwcaaRr zSs;PY8GSj_q`%N?LW)}ydjDZoYH#AXpjpO)h(YPTM#po%OTws4egq8ByTot^)>&Q> zkkLdS?VF!rKzaJ~3;ZWNcNi__b=R!LF|)^75Uut57Onc{UHF6&c3A{;zswHf7iBSXR_u}S-^d9hPh5jJam z2N=*2({e`Vo5*B@8;}($-6C<+o+bYu=JNZDKwBr~8hHB4&J%!6ywC@(<265>edp;} zaunZO@t~TY!+XzCVd2x7Lm55dyxR)ZcyX$iLti!Ho`58+WX${n+e3Avsq^P*yllt5 ztWe_3Y}Wc$NCb-ZM&uUPZ{MsQc zV?83tPfK4Y(D)yE0)`dj^)WcCwkyoXp?_+-v>u&o9xN&U^jTOB@zhN~4qq-WpkIc( zit_*G(T^Zeu=B>jhY$_AM1l7ojH;U&fzw-(kr(wG9l|?AP`=^PTjmp&NNy(r6!D|V zWMhmKY=YcpOg@vi7NlLXD% zV`nF^>9=#k?@YE?)zosZ7w`HvZP4R*=VLZ#R7pe_q+E zu*P$v>Pfc2{I)A=& zaU*7-MLv|IOrM>yeRdd8)suLzvNHiSCJ%$FO(5L700uDLKka9NF02Mr^?a|=?}4ez z@KoN%-iHKotPxogHC7esFUqH9fk#s%uXojmO_`2eRTyFoPVGzj)2Y+tX06j_%f=qR z+R|-nYHH22(=flfobmHm13<+6x{?I6spGCobs<^ghZ{WdPI%Uwu!805r`tN_vH*7H z^)^#HS?4?0wiu@?lswf%~c+IuCxzb<5aqw&$4@CAb%;=b?` z2h){0opDBL0YXppyH2${glD+hVszTqS^5WK^CD1E;CyXNxcxn8rT(|OvCu??_1Imn zt3oKvh&LU#2K3F`^kY6xV1gw_p%S|wZWZuX}IEGqYI2_2lv&C=X-6l-L zKA&0${u4^RedIm^1l`C$5qL@J)TZ0>urcjKIevRqh^I04+G+Vm@Aw0A^E)&>xb_N&#i+m3(h(66fpD#)w!E% z8_33iu&55_t6NRobR;2v(pKNHnqGC6XvfW;5#($~VWDw46g+X{j zYV%0$h#eft)HpOCC8kTJeJDAyZ`x+3%%Tk$U-2QJh>XdAn)-L=JtXZEIqp(Z)F$Y_ z>66$TK3UUnCvS5tdy|uOP7VEVBKe*l@8_{-*LH>YggVc{BbHdU4>jw zM{@}Kh83q2XsLgJi6@f;pE_qLY9zo`w9d#-#=x9-q5c~kM|_aDdh@kL#?p!ef^nqu zg2q%TVi#f)N||nl3)(J8SA;rd-v_)YQ}cVW)$A(~t%f^Wdi&L|sR`%hkhW;~j+qO( zZ^FJ-h3hZ~+I^g6tC1GaStSY&W8frw*!Ym;NA2G@c4L!WSti>`vnB8w$!mN4^kgZO z)|$12Xtiofwp(8Sn?3jR?tx?EYWlisT_2=TL`u@}0k-4B)u>s0?HhBZMs9*iv@b;5 zd|Of7#g;TF$wfQh)@gL5l8_;6hJtjp>ppEral_~m-8))i9u=_K_w$&={%=9JR^ht& zF*grj2^b8-O@k--skbUT-VFsdzdqz7^OC+%qx|~|TVDd_)4b9>!MO}`{NmW6J@D(G z@=~}bX=tPHUvQ!EM6TnRixj|K(){Q4LdP2&YtMqyu_m-!1V%mUy$S~}_ycyc+vG!I zSX$4PI&S8|h@=lTe4BJ+8vQ6pc_B;QWkdrPx*$49tXCZ#y|gAOwx{F)Qo#*g5kLBC zr>nNBd^LW6Gh)R&5#XUxwi>^}>)t%7o*GKLq1#lnkji@D7;pQ^C5pYyn6{&ZV5h6? zvbO%E0`6Y<&LSa6Lb{SFIQ=92#KU&w)^}|W?y>}>;6F#ypex#UjE)NnK zoPp>}5Zr+Tr{~dIv zG8*q8Lu2atGtSHLob}6Nn_-q%ER`kAZG81#y%ueU@bT?`{*M%#O=C?+f$ri0{$ z$@yEh@aa~kGn)=|l-gm|EMipu*1UHUrxfxQdhr@++D0zA zqOd8uilD{nNRrs*w-pbY&qUIk)%k6urG6Qmh1g;&V%d|m8hGmlAzOL!-+q*5@}4Y3+@Mo8W?G!%dqJw5wLC66#|U zCr(KT6ZYS$*irmh`nR_v0>TK+=x05Bw?mcd%TW)sb;Ta7?$Vy=C=Sz$zuI#@pS@vg;Lvg|##O zs3@SQA>x^2sEtk+3qDm)$1WTfHu9N0IaBxi&)0n}2cB63JlVwM(?_Xwzqn3X!xnq{ z>F5&qgg{SbZk4>0SmsiF8%>x%MWfS8~55w^7t?RcFJ7O$Doprsv54}Z4{B=cKukx#R!hkbnf~3vW=f9%r2b> z^CEI0!UB0VVRwi0q%3C?`((gq=+F+MGnk|JGAn@kK)y1~dhH?64bZ zg}ISaSoW-l`?i*M{zF=EtXCG!`?M;1J_}1E(o+pmv?0aiYu})FdiDFW&8*kneQ@;1 zw_OXTxlN(UAB>hilfRr-Z{3zQ;^kWChCFYi7a)1+!+Is;fEUH1#W_qF|o zZzM}fF6I@%h+68iAMT`PDasZYGZ6dED>3CWBepS(Nnoj5zaaSzml#5ORFQ5ZK76zx zNKu4&wD#zS=+YZd5BA}l{iw3?pYD%+j3+wFQM2+9v!A)CYMf6_u<1{=5zho-0=e>l zwYlTuEzFWtsMC4LHQ2D^iKOj?qNx?Ze#pye^d(|1+{t)K+JVux`0Ksy{Zi1p{t3Yi zjd+k5RsGt@hGh zYnS?5uR$(@{T9OivoB;P?KZVFj88T-QnC8r0!ptUl^ZcbRTrT=5?gy?bUSy^U%(0; zxpykba%#%Sp#*7S7fp*K_SZS^$rs-p+B|ltOVYLm3R%(>_#6)tU&`nkMvH}FGu-z?~ zK@&G=4sFySQLk31mj1~Rc3Yeap%WEpeDlA>ehrs=`~}sMRFBY)<EA`89XgdYt3 z*WBPT?|3u$*hb44+$lz677Jf8TtYP-Uetl0P3p;IHPQdMuCEmBdpd1pnKwjzQ5VPW zh}G6#MOOS%P#=kIq?Nv(B~74z^@=%L6w^!_2uguEU8Uv8Vmc&C6B^$YZc0VpTz3l^ zX#35Q;uqh~J_x8_PT4iynqk}V+`GdC4;p+m`W&4lhnB|3DlAwA(@HxVxca}BHYezo zIt)yi-o5o4?-N!@63xIB=5OR-17ftKDYz=|ra))_}54RTDBP802T;Oejl;G>7vIa4|r? z1&(WtM>4Fo%$vGRs#YZnjKC=+ti5vN5|pOnUZh_9B3>-x%w`q6VoG|-gHx{2hEGK0 zmssFJ<$LXOd|%{iPUk1oUz{?n{C7$)pk?YD^xBSg32pZL=o3S$vLc(6D^%*NvIW(S zKk?<3Y`}RZ%V7D8ntO@(?FzG@qX#}gRk5+B%498KZ`o4$laL!iam7}PSd(4_-jhdW z+25T_WYu3bHKJ~>rYEc4x0!}kP$o4*6U@0~o2&(o3F@MNbpdtNbn^yz z$?1$*2GK1W9XE3OIe7rK_HCuvXzDS2E225@v|zggELtP3*c88HhZMC%yViUT*c3?o ztMm5_?7uA2_9Ncqc9Ra+wBu7dl?M?f z{cNfFz6GnE6w94q3@TG%M=f1Lc;tZm)w^1gN6V^? zFK~hMg_Ht!T}ACoN!`A<47oS!3KQPp>nTO*ELxy+TzOXMJ#)}tUl?IO<99UK)%1aD zf9W6c!?jjx*yK}_$KHa$Hn?Eu?zbwj185Yu`%rv+J@t?PaR4KCaiULfHJ90c*d^2l zgR=c=WsYd``r@EizC0l2t-=x9*uk8a*&^GJf74wT5?V}Cb@#Mi*w`VRL$BukxHP=3 zc;jnbKZ`c`s1h}xqv*rNUOgewfKT3SpN~<t16OQr|E5w+ z*^23JzBL@gNhqQXr)@K77s4_R~+ zFDaB?h^Z2;IsDHuEh1{FB6!;a$d5d=A_3HhvzQsknjzWHiu^7#Rer94i(0_CwL$cx z2WM*^Tp7wODg~tNLnl$TszQY3{DP*jT^z^|lpX#=en3kNtXLSW+j&Be=}^72)w%tN9@Vw>RHh)NmS;zRM| zDl{+UfFe=xk!efeJ>D_1PvPpf)&vZoFXB&o>vds10a8v%G9kqvx3^`JDdD4Zg2v8rO6X+RJN9ld?aEi^ia%P zm8ydR|5K4n&to=b zVf`gjxCUsZDBi4L+cR!oT4*J$-s3~4&&5gyhfEoIdbl?`eKU+aEZMayhZTwHS0zV$ ztdo)ZgSI}Mw6QfU{GspV)f-#Sw+$N)|2zEet-*ZB$&1Rvg8i#kot$R^nT}n^j5Nc3 zBs<@&T&;(#s8TH-uzNRV>19S{RM%HIr9PL_^o$das)q9$G!ikF!$yVlWApt^`Nh>S z6$w!$O*;YiuZHVwAQgo*93Afk|Jo*wu5ynzOEt^0T^O@1+G% zfk)Rf!l4ph$c0c_lpw0a+{4eR$48D1_IuNZXDe=`>B9Q$|Kig-N2(cU z(=^_R{FSqP*h|gbnI}Q4( z8cb!G>>!bFXeS)7yG_`(RayM@dJFaQ49)B8(V*NP|a-M7bRbw2E5o{nc2{i#X z-@;xM<;Hg?9gJwC9eUCUv&J$2IHsg2fCfuE|DbTP(J|VE2lNALip5v9Td-cnZJ#OT zr+eTISpA?Nz@n9_0jh~-&q!Za%gY$r*%(>-cVjk^G+rhe~G zDK(-OsPTD|VLcN~$Kfc*>9DPna%k$#u=K4XB)@O|`^x5Z88ZMJ5!we?I&6cQQ*K+q zEv_R@be4sT9ENAJdmwK`QVmzh7Gu|K2SxXYraNn#6j%?2`mKt;iYdhw8TrSCdSu@A zz2OtnN7?p+M$A3%mM3%uG=HB21AH6GWg_4l+!%(EqAJ`KfRRG<9J~D9a-?IQ5BvCe z91&poJ4#i|6T!&~nG0gFEb^||r&XKPQ?&dOvog-y z8Gg`i+YqCs^8i)kbo+Sc;{lFKUzjVB9gWg0c{&UB-28k*wh*j_Vx=~0H0AuHSLZIf z*;PxH@>aymV$EWK!@A*Si;dlEURSD7XKZxds;aa0K%meA>bwI-i~)rlhT-Wj1Mfho zvV&R2a~Zg64u=fnTIfal5cjrKEsScrodT}P7pg8s$#QeO$gjdNvDo+y+^%Qg;U}#tapBdKrl5Vz!-h5x)pRe^Ob5dX{Km~8;}k`%py-Z z=^yRABkCIsW7ZuFCM!u4r$Plqy}6e9w)Ub84C{K;RIT#jbj`<4saZEis(ltg{%C2- zQHfI+*;-2#L9hu$f77mp`VZM#;jaKU@7{1^%$jB9nrRyI^!p!KT)DUI23A9Rm%SXF zH?0$9T{aElV!@YI=X>kQqvwY9i0UTpk5lvCZYs(4;!MdtY$elyo=l~cj42;G5b?OV zjX?3uzOScH;edE@MN=~wfbMUCbIu<$Vi$?Qu+ydTSgU1B8PVynb(xu<&&=mchSV6t zd1$KqJ$lz6j27?t>8sm|0rhMjzh1|h&Wkqz1|x=Avdli*|7fkd&{uM8s(!r4&9Px; zB74^;-Owe`Ccgfc2p)CT+mIgTR;UL}zhm>Z7CmZQ$#{SBiK;S5j<{%$vhH zdNswkmw%yBYA%i3BME-Z&}K|MGwtOJ{I+Ud8syv_h#o3g2TJ3dPOdm>ZxyzWejD95 zfe<^c_bmKr(!lqzypCB#L#J=nF&_p~(7A-8gg&wTldy$snmd~anB>%bgvXI6WMF!2f1aTYWW#?Dx77~(r%^AX58nc6;s{`d5m{iTD3vG=Qk z>A^aEjDRpkTC)VMp=j*GAy#(PmTDN{=Zk?aR?xe4M$bh1sZ?|GEiF(U)7n+2?}@jQ z>lTq23A`_3^s6bpdZe;0-^G;|q?OT{qTJq({HdkS&_f`jWIKN>y74!EL`mNwvW8+Q zvlTv<(c>b$T=Q~Ge3UJg^Z`@u^33OujQxeD$(8Sr776nY^&zf6&N{%27L0q3&K>DM zhGyp1tgM_w;yY~ZR_h!Nhp)dg)i6>sqwyfMkhmw~XCxXg`=63=A72O1}+qp8paR=zZ$K!9~_13l&UJe$Eo8^wdnBt*lm37>Ju)NNi13ss%I+@|H0U;ZHxz4F*VlH}BirqZJ(nKSW-{*#iA0eLCIC4+5M3 z^IiW6RkDT&SxMyS1skJJ70{Rb4Mg#Nv1sV%U3-Y8H@0Qd*K``BylT+nUlOpaT{?a% z(8dN+1`Cvs0!W6ud`vY?r0)^U319Tib|nj>_{M(C+(X-kQyg&AICg7W8_=ogZlMJv zJsp~%S0>#F>1Ff)hWk_My z60;AH^@`saO9|@s+!l{f+T)6J9!cc__#2_=o+mLC>CA^dphXH)UAB zwnnn2MDE!Q`R&C5+KS)_LseQo>*4`L>pahZ0kfmT%UIUuJ}-@71glL|PrN(ukKw$| z|28_h1Z{+&9O9}{ejfWpnx#R;a0xrpU!d29iI?SQ8z(kB^n)J=uEC}72|z$6_d|FG zLUcr2aiME*<3OKYCjCZYj}V^aQ|3IWHb!cm>f_kz`(sOM6w^lU=Ld(uu6V~c!mkBT zR7uyvD*7G9G~Dtgx!tR$V}Bl-TKW9*SW%5DKy)CQR;BdKVt8PuNAM;}jGXq+@uXnl z+U7;X+erh_CVoj9=k@I#3L0^<8!C0@MNCtF1ktZg-jZjS9q3)K;`6q^q`ytcMKfrv zjo{SYa9xhyGFQzNYu6Xt|o3%aUAhGjc)4cMIPx z_-iY51E0?oTHV?46N)Q(?e5znEU|5uAL{=(JFfU)Zn0$8Mo2}Zo@$VY8BZk&D2~2H z*=3>69Wg6aS^MQBmNCvEJcaHs>wMMqDGKX!!%js~m=f>Ck+Gd?QIP4*V6J6T1OPax zOqEHCh4(t^54xja)>{pT`YWN173hn)`fM-}lC`W2%{@de3$7NOH?HmneotP}pa z@>Z`@NbP;TLcR8&V4k6VSiEW4eVFXZOJdA_sr%|hv0_FNpSfc+8@(x zD!(w{We|x`ykZek!+a;Jz4wu*UL(~tzlwMyLtwIjvix{h*3NlCsk0dYQ{0>JX@KGD#J-!TCiO;8SO2Bf(}+awjk#}!ZE0C$jvcN=T_Wkf`=TNK0IgiE zc1tTECE)G+sv+T{ec}kO!hX9jLPxu#EF^zFAtQXHNpJXuMg`{`Ep#A;d6LloW1wOM za_(W=MrB`94PS9q(&YsAbt^P=s%05MkA)Tt4acp!6qjSnZ9?m~DERYXUS-+)JQ&(Vs`fD<)xlPYoGDsKC%A#w*ytKpi z(>KOiv~ZCaB@B8G`!J}Q_I%WV{;;uvcfAU4NrS-a|79(Vpl6b-K*GWM1KnpN;Ek*Z0D#|9vN?WB$wf8)Ozl}$4m~nfwt^R zx27R3;)r^GW(L|_`IG33;}x-=$JQjK5@*g76qf|QHBdlgH+D{NW{jA{84}h6vmyh# zg>dJ>iHhytzCh>W0AIeA)JWygwAxw&+JY*kBPCa#$TjwBqE0!IVvcrPP7U}-!sY78 z%>s#QPQp=9-u~{fPe{Y5D&wqpN1B9zbSv33RTMVfHIW*UkX)2lFA}dK?}-`nSq(ux zW>t4qxh4pPfMg@TC*fA{M_10qdaGvC!>`DYy2SZXOJTL0aehG&n0{1QyBiTx4VAN zy}7EltAdo5xOnhvfAJ*Gkj`4g0y)Vd$4vb4bspsMvjtWRLCE)dZqttiaDo%g9Q*Uw zO-Jbk{q?by=5xb-Jlt7+)#7Ovd_dnzcIGX_NjW1G2Iw|8!b$u|B6_73d?wU-0N&ZbF7sM@|9#* z=WjB&kkV;Hy)LbLvd6pLZ@EWC=(SCA)pMx!n>0J`88?WeJSq}DzO$Lo{8xfyR7jlh zb;Dp8tHB8AHkPx=504bV1@AM#fj$#=8Vto%u_JAP`d6C2qGoO??eY9$`(lx0vQUVH=VaE2{Nd_fvsZ#N?!6_CE;d@SN3)3fh#oIHl z=(#CR8R`@C9KxnUn%{F{J!e8;17`<CPJVi?PiVHB z8M_C;DnCcC*<%tG(esi)skd!UA9KF98`c_F>f)LB@>Wv%Xk%%Mp5AMtgd>Az^8aS} zXCHkfW>fYetm_LvG zZ!90*Ro?~YDa+;ew46;DHP0Pyl%Bh#^Y_5TU(reX2$_GVL=;NzcHQ@lr$(<cUwy3FXeBCX^npU)$a$1gA02EO_c+9Q2M7Cj zQ2vm0yo3>I5q7Nij#dGg75)$A)V8pXn0aO^SkXlx!pwDB(J$}rhD|Ay>7UIVDVL!n zEZ%Z=dDotAaSQdSBG>-fD4F=kB;u;JS$du?#p_g42}&}ywA5SGI4_!!)2WBdvfHi> zm!?1(YGDrP3%i$C(3kyE2iI`)u0LwBWNXSaK>hndS?jbudAMLSvG0EyhF3I*VCwdrN-E4~ zSBJCQAbBthbIM-B3Y*1-wvqTl!IVogzHu~NF)0rN1W59(=RkL}#}TcgHceaoUGWMh zgeEKxPq9m?F;Z2?4C|cG&yaAEeWgMAjt7%R^#;YqpGG>o7i{OJdvrIaQG`*S>u*yx zaiY(-ui;rn6X~JRqmQbC&ys!~Q=qzmT1Ojty~^ia*h_{s~odmhB60T0yp)(a}RJ6C0gNiMM(U>vElL`k9B%(d&Pu%&k&{0 zIk~LVgroD{jjb2H&*6=X*w{-kQYW5vm}fGlpHYN%=F(gm9JiK32-DTcOc=R>DU?m2 z#Y`~rBfO@XU9V&X*?Wyi91$4w8%e%wu#U5PfCL;LdihrMR38>%R@+FLK!ntg zmZD|N*UX3f>d;TEeS;`AoaDO^`Vl6@u<3HVBK-k7HV+7^G4&zjerhT01pcUAUO-S< zkGel7C-JQl?4{)9pHq`a;tNo!nacs&A>6y4tXw@(Kb$!UuQsQHS&mv$nRWcUSKUla z^f)H?S-OY5&}c%xKWThJ0*+l!gXPBt4(^IpXNc*7Abr&2?QPEcn?+<7k(Od-*+Q#2 zGF>y_uYA~UQ8iKDVV5$^&y?$NbxjXPdY?mtw6(x+&rF^-3V1B#mf!e<1TZ?bA+7>z z<}=Y+(}$5ozo)~vr84GQ#I(d0!}zy5M+-3EuDI7V zka?1N%X4w+d@2j<2%X+C zE%+)HyyvR}CFe7+P~)S5^}0UeLzhUv6064uEi}?y{C^Z(XFQwh`|mkjzgAVNMoUYJ zQX`z2RqJ%1s4BH1NmY#?QZ-`dbf9)o6h&2IhZ+$il2fxNYQ#t+sXc>O2_fhI{NLq8 zK6#Pnx$gVAzSnmcjn?dy$0SVom-NkBm3Id+8L1+6Nj|M?pQH=jWs^=y=m9~p5pSY$ zmX6c5nafPsP$g0!)3+l(8dPZ$ZC)1FF=kBaQlD6Ti|Ll+x>eYRL2&V**8T3rdng@- zRwn7xkR~saXnA*Y8l{&QK>ks%_N&PcBLQYJfF_9&8L zpJZWHH>x7WV9ohNR75SCn9MM~e5_ zaP>q*8cZb^q{o`zB(D=})rfjU+F!@EXax;AY|SJ*f3KqK3TCJBn}{&@8+8y**K%fb zCsDgUR36+B-5$Ryd5H1O0&)nQExuS?O$eD3Ex1&QMFe6&2Ly`;SfJ(nSb+j$=&c%Aw%ud(UIaB9--QEkC zFEf4=UVV3QT}|+xjEewJd&A8+G=1?6cb-O|KB`#<+K49ocy|Sog3EY zUWk0dmbUWMJuDvB2c>QS^!8Ffp`b2`RrP%FYm}-?mj^;WwM^A9Uz36ErkT3TY3+}Ia*#Y zpA%hX;Z1X!pRGuJ954G1s8bPUAcDQZM1*NjnR)EPG-cZ;1-Q6R zHK)8YZn|`*+h|a3YY-h5#nNk!x{+{?^qLwV)2r@h>^M%*yB-|p(oGQGu{5$yHwReX z5!M4CCCJPn-}2igS>?2Y{de(-=%&lLZ0=}sx>m;=LHtZ*+l#8Gf%f!e;E8`ngpe#) zp>nU@qyIGn1Zy^yS)vMJ1aSa3a%!faXcith!GK~W6~DG`5AZ0v1uO2mClz>}VWNWU z#;U6{=?biyR2S&&p!f;(9kF-Rq|OP2`kdp)T;Df>Prjr-c*K&wQ?eLFRL-+{jLU=^ zL}p)%s6TXT4TfG#8x8M`i|CDHz_PS|+B)MptD46qt`1er;h)oYF#_Mvb&pr_FBCw( zJ@{&9yFt!Lxb_=BSVx{99#{r|oa!(W-@lX#z@G;VfaDmSE8tB_L#ClFc>Z#E&KCXT z6mcK43C+9zrRsq8x1`$wZ=xz^z&_l5bM|te3UE#kKzb7FILd43x z6f>m_{KTrYDHJ7*3AcE*WnU}7G6!;+YqJtmMa%G6aZKyV@QSHm+P1TR^ zZStm=+k)L*y;#xH$%mGLdd`i%kgHl2rq3H|8ZG`ABuU2XHB}R3xHToqnGN`cmo2RW zf8MUP>%Ls&^IKpkjX|%C+URoLS&x>!1kePpeVV3Y)d;Ev$7UKQ3c=mbd+KGrN zYtNtaAJTCaQqj{uM91{z8d@FIm>ay7D~4UX4P>2R>Z?RQM&$1>fy}|Wdw%vSJw9)Z z;rd^xu|VQ*bx2frD3OMsrDxEnE!no5P%9ZhGlnqpSzQC~U3h*tf;Fm~|VRLnFXUh`R;aDaUJXf*ZOo z_&8{o+PY89@AT=g%o_h7v{+23iCPr>Y)Da7D_;JUjq>YzNu|fdOnIhoqrBlu9$q8Q zE5k_z)AdB0-G^OTGqG;kvU=Al!FT5tJrn3bIX3EBTPXz7x43GSfCoNxx2Na@Y_~$P z%Xust1A&iuMQjWB!&UvaQl_wFn?0(6YcES1d$TXBP`$nF)h=1~${_Yq*mlwr{~r|N z*|9(H)^Aq%N?UfJiFAkkw!uAKz#Tl{WbpRF1Ub&K3M53 z31OTJdT{$a*^LE6^g`cMnqn76QS5UzA#MHxqxF}hrJ?cK}E{KcrnjkG%VZ$<}D{P*u`b3vfK8NKjRCycsY1s~sVM7KfE@QvM z$24_HOAwRwNu$NaaXT1jeJ;7h_+D7bQ{dwVn+7~cPgzqhNGG!dBpGd2{|hzZR23mracr{PO-&B32|a&$UysI^qkNa`V*MzQ>3E#$k-eAijpmrw3Vp<}DG z-x|QeRZ5P0yn3CUt^a+6lglVG20wmHn2!{4`W4t%e?@h)^O(T#W+g#fMe`S4#Ut}v zf+LE6NXIBg!sAEu3ly9|&#r&@H4`NZdoWt&`UefvJ+8Y`726u7;tatN&zB0E4!{!} z)C3#H^c^{Jl?}USCDh&u(J3bV>m;OcvNW+1S3oLyB*T@N<>k@^Yxg+xuR@<5zF3=f(@gcb5Ynv4FQ=4RTGh{6 z$Z2fC9)=zJj@17fS01rd7fC16tIC_bnjLLtyh^s*$pVgfcc+4MJAtG}19~Rm0pGbA z;rIUY`J{OA4Z9gp9leSq>V6$KrP$?RYBF#yZ8*`0Lsv>~*EuM#k2)p~w1t^e7kQs! z9AbI~gg@efAxbJ^wkcU+Xzq=v0sBIiL~t&< zsLc>G9FvyW?3a_d38z+`d6vDq3DE=OXfD$2ary>qfp!k`YCU12Kla5Ud2zH_PMb$g zIEoTgZJJRNn)H#>`i@*^*L9lNdwr<9{NIr``#>Jv70-VgoFO|UF=}T9S$TdcuY#qa zUmD6zJu32F_9*_~ppKTEN59dLF#TsoAc4?lYVmscIaO|WJ)i|m-K$_4O(EdH+NT0S z%LQ&_Q*4|RCJHbSRykz5>a1n@Wi$N>-!c z&fZD$E1)fX`@zha1^Q^Mkry)ssMj{MXp|&8*qXV-xROsP9{Ww!3)cDh_t6T!D)HT? z_zlWJqN-N7n!01XBh_gQ=C%1tb$wOLv4bn@}zz+^lf$Z+~0|!OgR;87IfZ zPU}@HAs>B~m>PbX-tCf@M9vFfFGf8lndV$6{_lvj3OZ=zbKS%P_xyhr|jMfFEh**??G4&T;_v=i?{~&Z7oUMJH`*ZQp^48pqfUO&OQ9Ehr zzc{cM{ONkd|mq{<9QL%w_OO#%^_T5C``)_d`Al3-#|HCt z5_9ZeKjTTxV{>(Hx&XP(fn`t>p~K$k37NZ22~0wWDG)Mru=xxwpA`=9FxDqxI;{kj zql~x~C{KiIs2??YF0WDeo_aKWp%oq7up_#+GgA=TJP5TJumW7E|_Oc?X-6ObrPh7dWk8RmddxGOSq>E5HRGop_?Mo6Ch z`(4QZ{n010xsU$Sem|*<>G$hPjI=iEr(#Y?J1A;N>smm$d2gxV-b|trfS-kt7;J^M zf>(gGX=j~5$?K?I!w>oJ#4MBd0HP#AGCjl)HKvUI;0~J&6LsBGv)}=kE`LqoheNId zoB*)3?XujSFyzegBX}cy`Nb~z9PWyaIl}PA{Tguxx{TkFz4Wwwk#xL@)(F1Ag7!zl z3C9gG%TF|Simqlw8o9s%7H(e-7aaQ~nh|REHs)KRM$cJjXsp>f_GAfCYs#LSDd!E2 zHTg$79ItwZFX#V)YOs%wQRSMVubdc3dNM1`n65(_YWSdf^&QT^H;LcvO&s~piq^0W z_zlP>Lj}a`HNK;a8ATp0+jjg4%3%zuIYRb?iG}zvzuW&>YKCG!L{}|6yPrR1DiaRZ zbH3Dn+b%Bpk=LilgWu`44K{QcjJoBsuLQYbxOVHyd(~%NBEM61%d_4^-3&=-f0WK} zq5WJWXzwXEG$REjxI({~oghyuwrTbZ=r~9eu@G-qg^vFgb$x{?v|}aMft<2b_kp-q8DhU11bQzgHf1ENIM%s`AJiBd~_LZ9BtG)+ietjd2dl=V5zG!y& z#4=fVY3U3$-`=Ug{a%_l~u_K z1I+8h@A%7I_Lpw-uW5ZC-xu_?CC1_N zNVJOfqGp~{=~lNV@lvy;d8kFGV`qgw2#uKO7&VM^zJC=aG_=67i|GZL|Hwh2z2 z$pYc+UaN)v&Bz(gJ<rBWsAUkZP}hu4nt zeje{q`&T4wG_M@$B@x}Xnz?`ERpy%B+jw2W$ECSWBrL>;XbGI%*?aeEBP@t5Z|3%- zL4s(ZPQ^ra}OWxPzm_sKZiO_%(t3& z+031;E1n^*tpNlA48LPgwa68eK_Oz@&)g}BC<{an&z10KOR~8!isVowiLxhQx4 z>0Yc2t(}KGFp5f1$d!_~4g|4#-}aA$-l{@DL}3 zfED#9>9%L3y3fkP<@4zSrU_I1(UCK1XB#T~{X?f5>fW#r#rkj=?S3?Sc|F2)1NA$p z>P=~PAv2`>zazGr zS4rnhkqXf`@y7NVv=_^Zc1YbUotO>Ug5-@L=(07)(H5)U`~p!m{W{#M z4;0 z9jZO{8Q+oYrVrfg-?Qc~Vv!d_2{n;Bf;Wpc*Hx}H^RL!c`9F4M-tVowL9q@~ahogK zAq#TRE)AQip3QcUs-&ZP2*Qe;uOVBUSDfQX%klVG3# zYfw>vds=eIm;ZlR7vk>yAzyqDjRQa4Eao{Jq7VfckjA>YX540#8}n$8%ZyfdkW|(< zD9CF*;{KmiMsJ^`ZMj&WCKR&Bho1@M2Fe!0njbb*K5wQUmUxzFfzZMxHTtSyJHtIT`&AVLQ2-|T?D~9Iw?P? zvG^9TzJck0tD>k1l4I2Fe7_Z0663P)FiLAcdy*s%c zA1}6>C7#5%b;7wIa(eL^5p3pXTJYZy{cJuER$fK-zT5x3YBuuS8-Kr#g;1)fe0BIG zXTgrS>Cifwr4Qk2K%$l%iAVvK$I$=P@8vW3;Z<~8VG%bC6iV!cceKN{1|QjHSA~2T zHYUb1rl+2Uo>-(1C{B#N(`y`%XWDyn>a|E~dKPanZC@BXzS*MoURr?j8W*>D1 zRpm}AR*w%{skh(T({IApKq%D}Ui;Xc@{Zz*_H6XR+y*YY)JJ?{xzD2^ONIW`k>|MWMxRw9`Wv7?=4DgV1*`V_=-l-FeE_bE;=V=B2Ip5YVV^||2x9&PFd7%3* zAaN7viFxzcOQo}apeI$Aw0%(BAY&skup0||NYjeWL_8!8-$qRH+` zoe8r&P^Rb%8>^dFb>DYCrlMpihf*F%YL#qH!Ykxx#H;vWK@eH|>l+ziN~cE}H@MBi z3-#jaw7s}`t8Dl>HOy$mFL>=xSw|6e?|xCwS`X*kk{I6dWgx){ zr}NW&laKDlf|XJAO%)TUhIiSK8_P=Gx#S?MIPBJrJ4MDrH3+Et+6jg{A%+ zv(eMtQh4_1zaxS68S`&k%bakS$v;CR%NIm;xsHc4dXdB!RLANg6UDo>tgKN<$;{0+&+pz-xCT+{<%vqjT35AOFKA^aoT)ePY8nwx`Ig()Qr3oY`; zQ&63tN`jG?arr~0M-v!ZjXS9Oq!rwa6m6dRvwG?=cquF>2cPPWKsK%!+Ip*xH07&S(SjpM+3JaU0chB7+_|5YO$;HYhta*s#8TSsp;8rQ_XiS-*gfT^dit|t+hzO^p*Tki zii-;kyg7eEI4s3_gq&xT&pcExzZ!d;7L13Zn8<2a4QcKtG z;Nd*V^1fvk4WOGR48H7z24J1nlH#Q^o2z4Qr#GZMed$zWlo!%y_OiRG>Pyl;l-c6f*o1CL-&301|$e&exTN5HiCj~3;V?cI^%TE}wf_rTp#+2frNgi`rzC@~^8lG^?IC)CqQe@~7 zG_nF-#1iiOd+(95;aC^7Bsu63PWCd??&s;6?-CkH=z7V~g5BN}1L9T_$AOeK>nwfq zVh`w(RlC(WqqZv`Bnu>gdas_DxH`^sUNAflxdLC)$`jtSNGLN|sqo2K<|hh<#k62( z+}C~Bb<={QXYHDte*^aP|Gf0VuVa_OzP)bMnE!ZHNy>yTiuCTWDNy&HkZkO<9{GJ> z3>Ze8i+ITH1w*s3zN;Mlp}(HnBA7uZv5|sPxux?P*h(Ay{*4qR?N4*@DeDrgR`uTL z3ero`L9>24gSWqXvwW{7VG<+QWph5>{$-wqvm>9Ln8};IdXZBH`2LR zVF;aBelbWlo67T31`rK@eh^<6SHxBt;03gy9K@dQmFN3{;(v|S*Mv&mKF;`33dh41 zD~`p^J?`ELu9$0X98Jky4rQ+LrU&p~@0{yhz5WR8Z5OvWt;PIn!yR+=-vBd$jH#UZ z^aCAkS25J&zawnbX0*KflW5)QFMdTA?$#>Mk-1gNThHe%`817SBhNB_pNx~T)Lt=Z zdRqH$#E+4Jsp{u~8KRr(F+rySrO(|#{Xy#=<2dj77+sMIzn9)n|9HWHRFdtidLdY^ zM>!AA&ifRvKIKj2rVZWUe>W8-wH*&}vyM(E*VMwR(bkKn%3|O>&$Bf_eL`Y{H zyHT0|HRj#on(}n+XF&)jz`S`RAl1`?lELnQ~g6?X&UGs-LhkOCF;Px$lgHaD0V5x(g;I z^gr&Oi|e+TPkVICYRD5+WMe#Nh$uAEGRWq1SP6z0o|5Ak<<}0>D?-Y^)(+{p>J`_O z|6U@>KFb&6T4B0m#K&dIUTW^!EXLRmKN_SX80rxjQ_OY`8yAUn? z+z{H@p-&h?m`Le&-u!ZSap!Sce1JzD0b$a|iK`R|Y6|!B2iDwLDXXwkO!K4qYq{6# z`0P@W6xUEs`t-PINc?7PXsvR<7}uMefQ5BPT^kCV+5)#JP~|yY8)H79SBJNk{0ygO zX6>*pl}*+Krm71c4<3%m4zlR1_p%C>Ux-F+Q9_ghX9X#mV<-;#f@YR%=%My4=P~dP@VWdy@Vw7&2X&tu5{Wo1#b#C+#Ya!0FYXB8g`=>-Vp|S>wyb(&8~G$_Hsh7?P26^D+Cp`TEPlH->ewwsqMz z^&7%ToSed=e*0slpSjZ$$Z5-ak8#a7TX!QrnmU5U@gF4twtH`hkIo;@xD&` z3Fwfz$C)mPtA7$Sg%0` zlS3GkE+G?zEgUIW!USavohHS*jZ{ zZ-jcWs%&OEXK#ErH-7=|_1_UOeS?aSC#KFms8&Eg+Z{$_QYREV+o>is9^q|?xhXMKXlg~zd67}}EZuisx)a(m$B zoobk%j?Jmw{T=-tc9QPm8X^P{MX7XZzXL1qr@Qlu|>zAJ@ zQ^JPVzcz0q6WMFQlprwphEqt-+Fr%H5Vgy;z>&LC@%G*>_*_F7Q&JoedGt5L;_j%v z(dXrwkeFdhfL?;uyVI1b$Z0X zR0&jXG?ky_KCO0{_Pvsu$pGn={CC7G|4v3EmIFRq-ftlZ9$e!tRX{>e-~a6FZO(^8SiAj_2l0gWD=Qw07e>j~Y;^Ym+Fnm~jwE`!FXkPsbK0$f7Cf zIHn0ti-4U>gXs1fEqz`(WafOUP&98gTeWp@GgI{CZ}rXmS|y&GDG>3B(!!^-gIq~N zm!8t`Kd)S=kev>U65{v%Vz!2e-__<1dU1{mamSwg?hBu~A0<4*I&cud=gTyy8|;kk z!8o}jm(NOGaWZI2%&`5$#^6E=P+5b?9v zP|%%=pO(*u)6n%`3xF8QzV#b1lDSB^Vm4loRarcx3~vP)rpMbfXZg#{0?DaFZ~bU9NrVRL;Q zLVmkWycaoTAZ&H6TVq8>Vm2x0*iwo@FQmM@B|5bFpC~miDanofCBEzioq<=)mjItq z@jIgoP&fu21J`N`v?cawx7|rv)vHGnAX12SfBb0*8h!Y6@R9#)Nn>PNnksPx_AuPctxhElSz4MonYkBw?*PVf z_=A0+Giwv(vuU169Vnyg(ZNn7>1U^zL62V?559{#ZrO?iB5BgOKs`@;)s|@I?HZq{znG(jNbnoR#kS|Kc6z~#Rm97A6VW38pya1FhUH36BV+l}2RGT!gwhb31-ll~waOg}-#2i1=K3})r)i0I zsjmaW$sO&Qx1F-$VBwtoT(m>?EeYWvH+)e;SK0RR9O8Hr4j@6W13AguOc8GKv z!!M&8d(gmhj#OP1O`dTB{My6DL^m+LQHtE*?|i9IHThrL1voh!>Kn_NyzbfA4I!RM zi_UJ&hIZTrp~HbH6s@&F4=ws;X)OK|T!zSo)TWUSP6nM7Q7ZXgb@uS(74^b*b_Nym z!zLP2>#Yzeo|P%vGno~LFyZQTVN8$e|3U|S9CQ86Jh~huI=BWHWU>S-Z4|5cgUC=s z>%)Rp3eaAib*p9D&5~p(2PVyrD!&;FX)cbZ3$(kS!&>f52_>dGHjqEP5|&#g*$1R# z7J!b6%HbAY%mny$*1MauSac9*U$zZmzvrB-AUTY<;{jwL)4$yj7bsC3v$IlSI;@Jg zW;FxOV{%z`gZjpQeqMr{F_e3Qbe>Ck5M=1Tr4uex^J@WYmmGS}BH1ZwZF!(>qpD7- za?`~-xOR#8{n(4YKA?#{5Us_A1(|%()r1lI_)41^u{4z8O1>m{1Z1b8HdEW!!{2n| zR53ATUqbvAtQu?yL~(#Cf4MwYgf9v|u>5mJrW_VRZL`GXudPiz#YbPFlP3yoEuAjF zuUGsk^Z!kl&_dQH`GN`#_V_Re0BhGhzQRQA_vX!PVSsxz$H+Maeu2QS$~LgD=S#;+ zB4t&2lL-IsbCUAU@0w?Rc-{cH?Gj9bV& z4g`F?zkI7KkSsuhYp)!$QYj~QDE3NJ|99lnUpvM4_>)*I`x}pPaWQ=x{~fW&wd4Pi zneyZ>_kTB18f_j#tb?EoJHsb}7~3iG?QW!qkh5DzE~8*nrs4R*nlB+bk2dyw>yqtu z;2vZ@PkWjv^9~xGp_OlRaK8IGqw^G<9PyfRs|Hy=GH9*dmp6%zw}8)e|1h zr0e^>VU}z!+*L%lX>wDI0df+W#t!*1x2zT7zS_;j5AFV2G#@CwZxDk)&O*WWg2WHD zGG5jcW>B9z*ce1LWP+t>Dy|7hq8mhu_Eg_{3T=iraBaI0%CwX6CY zl=Uov+8-MqL(}6#K(`%PvsCDXx9t4;>B*p5`vf72!O%{Jwk>?qHvd-E$k4(MB z-ycDHrJ$ITlvcB#vigska@FVA3mt&7+O+#xDrhp($2Y#TreNAAtU8;nP#Eo`^dbN^ z@PeDouDiz#@EEiRt6g@SrdU~VDv~y&S-M}LuBdjBnRY`O8$_@O((=2WWOg*fZZ z+0;k2SUuidL2`scmvr5F3XTSg-q%>J>s3i=deXHaA*VAaxK7FLs~*li31Spx*MJ5Ux{=m z*V}WWUpeY?&Tm-}i~YN0^xN?F{WnGfC<(rxCF}(A;9rIsZ`>Ph8Qa*LtL~GE@Pj8* zo8n6=-F_1q-1`<`+>_2Vz?7)_yp4%JIA#}qJLOsE=ia1hV}kDg7gRi`#n=w{M<)$E zJb#e+)#@m1ZGO!IhB1$}YeHcrGl*uwc&5l0TP{2-{ux)S?PqsmBeD~Ca&J|?r!Rzp z`6uI{{g_V7*|!$c_i3CZ$&UZgms`0;mi()I{~bA67$J@6WG?rr1$p*4)Rl6cE=V#u zdps9d1f{$9x$0e;B}n8FLt2T>T9;F5VGPct0OyY%0Yn}$Q;btY%>Rkrua$p_av z!p3a!rH3dEV*ZE{wEGflYvJUanRS*u_>=puIrNmuWF?UMC(d@;&soi~`W9^)nIpQr zr(}z-^O*i0c|E#f7ugv8fV!-3jwGM<$YlON5sk09R8wo!Ly+gmkZ*Pp(BsY-5ivGX z;>e+*@s-NB-Rhcl)tX@Xvn)Q>yMQ&8IBBdf2l0U#4{Po=Cu{ZDv{fg3dQ@3(FDZCL z9(px>vvRwR;-{rz2G_J-%uP)E3sXUcm}~nUwL|VyScDo5_@hLG8z*H_`OnDagVq)v zYKY!u-QZ+B@tJEhZu#hx7CuXH%Q}|sU0A7-b+%K#f%#F(kYROO|CaO(-b6))HqLkX zZBv8BJl^6`+O2=>1~$gdvk7fQ&Ks5o*6sS-F6xEE?W&v=mxe}C=8elnS(FcJk10?u zrj32FOYN=a%KYw1f04WgS^>q;kIdsAaA$t<>@(kbzDz34`wcGg#MTSRkGw~Vdizi< zxTy9V()-{1H4j(9udy4Ol2@Lc;;^X86!zn+x(c{ojZO})D@ax8W%L3kXwD?j)xq~up>v@tE&&s4Xw)D zS09*1i-xTKd+2nHeScTju)R4J{c-Y!E3_lD9B$8=>#cMw;y5W-Lr1SP-Cf*k9<-ew zmOK-4|EW*nC{7?j`ds-fk2(f+GiQycRa_v#FxT{X7bK*%UN7u!Y|S2fwI0Y+^2zmh zcR)i$4ntJ6;g@?b+s3^6UUYy@IeksH z_?wq85?~u~=W9aT)eo-+1lBKGS3)aAzcz*CdbJ^)1^h}ul7VQJEjcv6tRlS+6N8v+ z(i&hcEg?0UJ$++Q&?Lx(NN?;<8Fz0paGCkKTL(!iB+w`S63X;ki(jTTKb<8V{vVGxy({k+lRyDbyxMWRDa zL$i=Ixg*C}(YcWM*~Q!hg8|$)%bwYw1wkVFz77_txQGdMu8onfp)y=bq@E6{%I6hR z1G2Q%7`p8HQ9>J4C6&4F&K7~n)qr!&W*Xn{!%SY*mp_It zu8Ayffvjs@FkF1CHsZh6B=!ZVEPxtWqUd&s56+SKp6~A`2d{ZUltdf78m<5_aL#j_ zFG({5-;t(fOYt@ie)AmJ^A)B#-|&BM^2Uddm3FC{P4E6njLbiElOxe0zWbj2HeYn( zH0|%;-UyzwZ+xOy9ZAHCypeoh@GVhK6K~2A)(*o3nGC)r&3mssf+P!?m|P7SFxf%ACFMWBW-fdk_DW^X>v@y*gz1ywGInmf#yCQv~}$JDQiN zySQ+Fbg-ZF&+&XG{pt;t=ptWuXva!m2_+Wx$&{0ZluW)@!f|d^e0e*6!|rVpQY-*= zg0IN9H~l~4h%%IF_#Efo{*D=rO#6y~XV>6CoYp$LaUC^k0U7r0}Kr1b~!jCkYS zRd0Ok>QjnbbEBm?l_G2N{UV;VsAbJndl)gf*PEp7mNKe zPcpAXZP{nFBi|xDF>w5C_q>t1O5oCWwC)993kc_RQ15{71Jd=8z?Cc7%7GJtb69fL zr$ZKHB^{75oaUmTjq#Tv?)Yn!w;cvn9E6n!dp|cB|`mQ}Ie^hOkU)MEx3MNUN|CZh?&C z?G4YF`1KxAV>_fob?%`Ni^OHT2v4)FB%s0v=o&~~qA z-tb7DQ9I|AHrYb@JnC>Zb1AsAzs{9p>$U{7mle8bnxDFSPxN-f+tIEJ>T;3~N%^0r zd4>)qyxzMm-nWMUEsY1*!vA=@3O=AS(dL*@Cu3wiN!ftZ;#p`cm^|stStKo}zkb?wxS|hMsZqkz6QMp}w8Oj`bbi2_bag~4!Lc%l-Z7el-2A(C3k0|*pwQ0! zO~XSBl}xqzFx!S^%Xyp}c_I@fOgBm7ytpH2)O)yJRtn#bPO6uN*EZLgrT4{{%{GZo*0sf?tavJxfl_BS0BqkF*!&kQjX7svm!h+q6RuGsLQ@VYr#jRKsW{<~Q@)a=vICp>as9S~FHaQ@s+c2wV4P zNSe{o@wvQJU*r{4f{g+;Jp$`ND+o82Cbmb#lrp5iQ+gZ)wHV;{?shyqu;xlm!`?O+ z%BKGtr9!0Wi@!(#?IhyMucunLbAZ`OrwB8zT;ye$xlunvt3Dx!LctM_^0el=^MyV=Fme zF8WCwTkQ>N+fv8;0nDT3BPX|f27blci#Q+ zZHInmp$I=NDxbTA2G4GtHcS8>>dS$DelAVb}$fc# zbvJOo?onv4ZsY0>4#ohh?*~}=7`T$>=;Yfgfb6>nNr;a}Sj(=&`{n7mc-yqB`lFnq zs_3$NWU5xbH`VW3IS+{U9zB?Ekw7@>>O5;Y1V1)0nXlQ^tbVpPH@?@~2|d1nnQu~Q zF0Pi(E&XBlHL2ln>cC}~Hl_BDmI6!%BA(tj>9i(-X37R>?8b7{FcFTVT8f{l+;EVj zb=|-}I_u&*EL6y!r#)__S!0NgG;E*xqLOIDUY@FmPRJOkj65r@(f;JM*=po@ObGh^ z;7tC275m}nhFTAM{kS=cHu??0KhO2b!PwP&U*h&ozH!F30gp$s8s!Xb_+4bJtO>phhSA!E%yt&RZ zci3|++q=I)((Lkq!wE_zKDnp1n+&Y}=XO_&+vBXimO(e|!{OTH>~))vx+$x7J4;tqSH~WiSSg{E9%jLI zlS1MnoVgS?f6Vokbkd*c&NRH9pi6R6zfFLRZ66Kh#pknn0!k*$iglV!#<UF~4RjwC1o@}Rz%Wx#!{0K3h8b)YaIk-T(Y_Z-; z$I(>Ld6%^2hT!nX4FSw6kL6*LZ&;n-G1X#H&Gv&JtDfzvv)BC0nXVl{wZr}_C{%yy zo*4fMxG9Ija>1qr(=B)B$JgaZ=>SdrW}6+P=rdNZx9imV@w2~9Yz^x-8f#Wm%g`rl z3VaYPi|?`fts@z$?3enXazQv{j^K45o}+AdIO>$= zSavY=3cTB`7nk~g9ddSY*H_Jf=}MoPGC+OUjn)`|3lQ5k!ht7hj!0Q5V>8RE^} z?#Zo`x7!~VYBk+1!hY5u6Z;59+wNPqUSUwd*nrM^)&@#DbnvokSiF5BDfJ0VRRGI` zGTKoxH_MTyj{G*=TAQG}`)yCsoi6V6{66?TO=H{9@_cMz*pET|s+e(`(1p9YW4eSF zTT_+`EWh;cPEZayN$%M|Ua3jszU}vsoRq{mZ)WrvZcib%(Y(6heN@u&jBr$Ww4?hm zePxbz(cRhbHU5VnbY_&StF~ufw1W9xm8q%h z1z(eM)laf#;#^Ih#bVVI#QXzc#X1#Qr#XypT2c)~%1htM#vd=obh+^C82@oN@=F+E z_=$idN*BH|9WuRdB<0rWhSAjaeS>)84UnK^qN>}GqDu52m@e%86x@WzoCR zYMZyq89#RpisRO1dRgJ$BU8F_Z%JPl|2Sq2WjpXj4@vWb}ZCr{P2Y?Zr#gU{~c1>_u#c=7q18f z3YbYRaGbFl>863f^IbZ<2Xq#6AtnRdsz=ov!a8Kx@FGkRo+>|Q198Cq;8p|uz?qc$ zBU~B{HnJbj8xA4VxmW;B5QtHNy}ok$OIwDVb(45L2;75l%4&4Eo(#oJAMs8iFG*c1 z7;qqhZ-$;8fvyIskUkuUm}{vW{YevgOL>a?%)W;Rd@hsoBOpnOHwb}v6@uFDJ|A_QHnIyt$4fU zoPmic{f>tAcsbQL!^=;?V%yMCa>JXFOlrkU)S!}AqV>F0#;3VCBJv3k`qcv8*lM-F zP{F%CaW@4OAJ0m0o^ab94@HI-wn^3{X%3t}kO@6+2TV#GD8;55%~gfVj#~o(pT79m zKX7!6#^_~FMx6@q)hXCRHOfV;?3a|Mtz{AgCfb@eV6m?qS0kQjJ~mvBc25WGmR1xP zsw#gP1F@Z6%gF`|WWlvs4QrT6cPlZ?Fsmlj$bFuv&!Y7vD!cr{9%x5g62Odx6_JGc zt54Q7cOU2J>F6166z$oGS86thED7KLNZ54j%2*wzmHj|WrET4*zcUsXfXcl;Z{~z+L+Py953<)sWz>9US@-S4*QZC4`DBmVOe>hZ8 zB|s#M4KHB;x6X*c&W3acyhD1HhIUw!W$1nh zL(*^bI|bCOY!4N001NRtiVKGlD!{2KcR$R|k4d2ko zvT_|&r{Ug=1{4-BwjCTSo~1l8*ab5ZMsRpnm?`{RO)skB>@`@+MM0sKiIWh zZ{%oTL4j{#m_`?u?iv?W16Wf7!EGb@pB&K*YWjya`8|o~+DK1)8~+l$Z&P5VsB~6< zxQ%dsM@hc5c=f=c`K{NsMDDsnUERD}np0xNJmJn}?5j|H?RFXv`m#XK%E0SdLHh!%yd_uy;zd!&jA}-wCT%8_f2%WP~62_p(T;3rPdk26sG?(Sd;$i zQ1*%Ft5ZkH8_9c7oHiF1GjvY_8606u38Xi3zsq97O%zEPCYs)cU`umXmmV<_m#X>O z6sWgncpg*GM4wiYbu^IUY8Y1@sk^lcQWcN){k?8gt4rqB3CZBY;&uz*0OZ0U)uWn2 zrS#>Jy}gt&APcUNN#GCa;tPi<$*+OekHX75%~btX6s~pDxOYUcXKmUjc(IKZ2?_LX z+{8_u*#@xzpxrDbJkfP zfUGUKa?^Og;eq+YjKTG=;aR(?M$&L0b1;ppM!_-(H#XQYyS_xZnvt0l^ap@>9Kb54 zx;YlT5c-?5Lsff}BTlvIK~D|p7oWHGAHxXBD_>KU(zMH2_DVT<*%kuRCu+unvyhfBW^L$60hI2mv~ehQDiSlYs|gC#(Mv( zURA&xQd5Ed3(P)mrPapwwWB)DX ze59e@oU6IP2w<492pZeZ<(vfC;69O7mR5V_q889UIU{oi7fDg8<8hHD+$Yn73#K24 z>8~>uj)z!46k|!~lwXN{)44Ha7A(qIV`DH5Y%O%8JaS`LYB+rQU5@9+diQhyRc=h^ z5DbC?i(e$YO6!Yrz+!@Wvw-8LwjUbsr+a2cjzyqmPSIVqd$c_@?Bz5?%;n}DOJNu( znH$_C(R&r!2)lqc%_Mv;7E2nR49`}MG;0DtEC25Z>jZ;3#H1R33U^qFI|NC8HA{ttcjUg zuk-8;Pe(r9JZ{da2rD+F$(gSaA{hil(SV_&v${9_$Hh+{`lZ}M;KOPxvKyrOIkS!-{*wKNy~dd$UsxUyIKp+B5gMJNSj7&oZu4X zgt0J@joa+ldboaMnKbtE_L12p^6w+*Jm;So+s1jI7To_*1uwWCxj{B^uW`K?rekK> zL*+=~|0 zUEPb8g{F1!pS??7Bgq~~)-5nV8;h|P)gPm-zX64ecS-wZt>>=W7AGeY1ALU8hJTiF ztXj@F#qFp(y3NI^NQwM2&Jj()7FvX#-w#nHc||E_Y52L{ng}#p?wH+oq}b)RCG=WKzNLyAA3F2rnq_50Y<-MV zzHL=6maKdYEI^IWOoEwAidbjP`)2sK@s_hBG=7pSC5wuSr+`%G^d*@XY3{D4MC+xZ zVb=|Q=?$>*&j>;?uu=&k+?8)SMah+>UMvsBxg4vWg09VlYW}4ov+-zg?r+OczioV7 zM-g;7BlN0JgJlGBO^wtnE~Ps(Yuw5$2E z^Ng_{B|hn5-9ceXrgprZv0)r|P{XFBKF8r*R`=2+MgC8MTkLERfD<5VWfnOzh-u~L zs!fB*#;M1-PhG9?z7bbYcuX<3tj(6}EZSe^Dpu(e(Z212#)S@$ik4_W2D2)S7R9{c z3cKw)upm6(RC@HrbB$__%MsdK_|kGQK+pO{KLYO}`#@Wv_sUVD9k3Ayy)n2MmSTbF zbDpoU2rqB|HoI8qVaiK;BEM(sy>nO$ZM0NJq8;-kB)0oHX z^sIhgjH^fOkt{E}k{ z#B6i#DC-9eLlhD{K4_Wm&9jK}eiHyrG|;Dsd5}|YGv+&&ZyyZZY+Th`-M_&TnwWJ; z?8@OTJaPvohfeROYmeO3GF#L+jr*qz;Vj>A8&+v!wC zG@a6+?4-#Fy_~0ylmQ{z>TpDShaN{%LK7Q7Ib?s;I@F(=SG`fs-dam<5@^4$tn4jU z_dF`kUgE)EjbUPN1k?|mo@`JGxDvbuTaalBNMPuM0WZ=P_y8TV7ShnU+;X0yRtfRF zkpgP-b=t#KNdJSlUc*yAJ(*aTwJ!E^w}}Uf-EJNEq||;&xOmL$~>&Gci=+vyZ3D`t7pBU z_Y$_oJqst;QSZgMvldA!m)G3}N?1OJD}H8(*Toa+jdb%5KyCLQVWlP5Ekw`H|MmQk zG??jHP3a=D#iJBuxbS=kvGLo<@PCU&x25k?(k!){|C+pZmdm3$lMRx~+vJL$V7y^$ zg@3Uzcn41uv0;E2sXI(i6$zi!9Vr@#8yb!jEpp12yNGbm%?<3SK%k9is-SV64Ae(q zkVU=*866J6WMMQddSugAya(ls1RCjNS&RZE^DgL7w)EEsO>I{{Xj)nlhvgo%ywj*J z-wU^oTf42|#H(?1S-et<)Ry@!w8;C8ovp=&IU93oi{V z@Dxt@%CAl~0uYV|!-zU(=)on80Nw$fw0C%FwgC73?9w%6L$2^(7_v?K&=LpizVO?A zPXH%_eRAJp;`To0J758;`fv!)^_A?GWsn%~+4NmMt>u1{U{T9p|f1 z6!xMnw>kLd`d9_X8Mg7MVeBK7B{dcMnH&@HeS++*R$LoxHhIR#c@P~aSXtqxC95rP zi@#zYpB#JG(T+q2|LrhY`*08_r-Ao$_~YD1ExoRu8xe#jpNIW-lHXpa!^aRuE9NFO zzo3DS;FnzmHuhr|FntGJqm|-|Ns4zKvsoRELk^5-x7gxgPBG6b|2f}(d(lvP-=P6d zy$^dhJ^y|URrY@N_w1%woNBR#O$ON-4zI${`)Bc46T3K!t- zKJRrIZqyOZsuo|o>e&AW+{C^u!hL~2K2n?&hytu^Qh7FR9FdZN{>+1aID`SLsNcwx z3o*MOuCA0{ZOj{BGY=hEQYSZ(vhk798?4MS^Po7|n@)?n6rjEQ_dC~gLBFwqGG9f_ zLCY_ruZKAag7|HOea9uZ&3b@n&@LjLT%)J^dFfE_qXWvs`|auP z&#MUKkI}^ByfC*mmN=R}P?w?*U7^B}jV6k*gVGws(73J%Cc0O~VQE0ak~o$)s~gL# z%Ny5(NxPcGlzK}JUuVU2)cfVobZi}~ib@6C<_BdsEY^MGn-FURPtK#W< zkcWFdbu+kXr3r2(*dm!}RYTZM+kQkldJ6y4{Wcsej5puc8}Gz2ODhbk%bne;M>yr< zyf5-fF!PX=&!P-}P(WQ|_edupPGvOovTwJgp`FfaQuei}4^#Rp7xecFj0el_}<>a(JX%QsgzG1$myqPC5FVj?Ms@*)|Q`*?NiYH85n;LZfb|f zq2%ae7jqd(%kr2_65OP78G}dg9@ZJG*fXfkAp6L#O?SM``W03dWLS0F=I?^xO`#H5 zfe;CWBL$%{w?SVd5>+9-ib`h=VKr#gN1;&$S$W_eNzMmKM3f2 zMf)x4-5(%J?Ej2*;s+5!=0+h6X~yA-Hz&B5S@xUCIa*%B-&sJWY!T3z&eGTB&?Q8x ztR>r4-Spk3-!-Qh&8TK+$5G6~BesxUsXNV7A?_ZL%`pgpD8pv2#SMs@(*eW2b+yd-mp99F1g4 zAE(=b1?2H;@bM*nrJ$lxgtupMj>7Bgcx*Ms!8LY9 zfa-_jZj)cwagL8v(g-t%``eZyGz>;JzH@-BH06nZFjkpVlnbteJ59txy=)kj-)p4AuaY z-cFQZJ8AsU{^7Hwq3Z|NDuwq6`=xhQsJ-^Fn52GMMwJt=AaY}Tv~vOF-|eJQFV3DP0oEj(8Aqy8`C_NZHK_2u&;ygw5Tex?kP-M?>I$) zunf?OJuyY!idJEK3<*$c)K$Qj$jS_Qt?iUae-A0>m^tQ$EGZ~YeMEQ)&-eLRMTh>` z!a~WX|2kngq*i%pZ_z4dX31noFBgMw_ia8i23qsXu%U4RF6-te-ORkI#SP z+2ca6wZ-?4sPn=}XH*M44kG(9wX5>B0`|W{pzco|Z~mSRuj;0=wN^t*RKUr^;BOE@ zWf;GUZb#20m8mATRB~=NVm9(G8{MthM{*cYY?opdBV-k@z>sIa}%l zxAP6=_`6$+2)K$dAAywAiCPprJS_YLK}Jvf#0T;If9*XEUn> zAzJ;~m6jOMuZg~(&_?RrDGoU~LTzIWviS-?yJxgQ+HtB)MazF;XIaMX{Pj)8n){o6 z{+offf{O+#locGRF3T2aD;Kpv*gnvb;ThQ`IpaVQD=&3FzK7vj43#?Pxuxf3@op#g zLil-<)~^%F3kJEUrXae3+t!#8NIEcMI|6;<(rZl`LD{sa}DopH!r3^sf^<6{TX< zoFoJPhKu=;dm-`xH=zd;T{S33%2F9lT)m89mMKr{{yGsG%yk%$hv(J-Mhtx|{{uX^ zw$z-iGF?yet_v+wb) zkTsPS7GJ5=fMexYH6-`($)mC1P(h8=A>$(RD|IWwPF?JhNN2cwQ=zrj4WGPsS_bs( z+~4Z*ieLh>h`&!0i zT)A&kSn>L1XQ6DTz=JL2o0FOKZD6_R()7~G5uB5fS~IausF|4i?wu;p!8 zKEC=CYENJH2Q~&fEe$vUlCl)Y2f)3r#x<5$rVI~T<;;-&_K&+_XFL#LM_F5Om{>;# zJ>ZR2WH)Gz1>6LYKRvW4ad05ra@qraEt^=fsx>zbE-?!rp1OAKtsSmu(xx^uy0cT4 zT4f>DcUiJi`x;~$N#E~$Y0Skuon8t5jR|*DB8W8$rKGJK-Y7-1V1gyi9+=K70OHom zhQ1}WVR}8QwV%>w3_%ZLq&0{)nauaQDQ}l^1tJaT(yv1Lo@*hEGsmTf>>|`m*+i@8 zi~S2qLi@&Ud?})Jp0ur^+{H>O1DpCaLNdjBQ^1YZ43cjsb`@~iVH83|UUF|SoB$q* zn|&DJu?{kJ7e1)b%hSgbH00k4q(KwfLhYObtPMN zg$k@boAL4rBvCh6XU%7n)JOEgU|BA=`NE|EMN-i2(_=P7VG~h=5nLeyk-?jog6CNF z1xK%cemirmnOO#>A;8~G; z_PBybSY^Mig8U(%`Z@A4VI#T8O*;&0nm+z51NW&eeR^WeI5Bu@yd-)OQo{s>u@Ms9 z!Ce6(h=W*FhF_zrGuN^0&^7z?I9Y^8d98GnwCM(xO$f<@h)xqDR$2?9M-j( zG0jOz#xL8h8Lp0E%oIv5#uJlMr-u-#7d<9r)((;fMhCwz82weNhy--h9-$KG9C}F$ zntES(ZV`_B5;d>y_d0jAX1Yh!))^Tp@Y*e_w|6Aem&jfNThtf?q#Ul@0hn5WD!@(u z(SY(LaYLfZ!+muNy%tuA33xa9GY|apGVCtT^XO0Z&BTYBw!SUZ>#-HK;U1Lcj3AC+ z1KRdUoCDHG723FKU{OC}%e>nDP@6|}0Id$2O&;|`BM*HT@Fh0aOIoO#VOSdqya{0h z@MkPfgB!u>yg5_f{Gx?Y7+$vbF6Z?fhF9c^;CzOuPSqPuKt(zpng8`m3BA9C<2aU% z1q9Zyaunyere^ZnXryA&6{4Z-F)mUYJiFRB3VPOcv2)qZ4&x+{(XbV%RaH|SW~e5U zi+z@AGAWKz8{f6Arw22B2)AMf-}?XFMa*xuX^qp#tnSncJq}_HYS&Tvj(_dL_QrK3i?? z=?0Oa-jMn@BW$|h9s2RIamB*~h-1SRjG7Q_MSW2hQc|(Q!@RLitasL}GmLA_Pu+9h zXw#8_EyGuM9bkg?>d5pPe;M2Tq3T_qIU8KAuekGhIv~H#+)|;THmfgR6=*_pW*T9^ zZDiN@>BhSipEQ~Vsf*=5O5E1I5Kwi;A0ls#$IJGW!c@cKoUfFYhN?}rw2nM#)84Ck zYIje9eM4$k(kx~BXfTz=(v45QWV)rj@7K{$ZY!@>$UI(kq(Q7#$AKva^D8CoL)4PT z6KB9IPc^vYd!?zfw8?s7pcZ2m5C^MFPL~}kE`P9rq=-|H7gN49?<~axicr^GH!kr0L{zaqIEPuPD+* zAKLJLGlax_ovqT_h;Ok-yI^8j7b4}rM7mh<#@-Ba}^fnMWh#HXL* zad|VX^jSW+3&&ZVB@sz(@9YhJx&*@BuZ@uvjn2flDqQO4MB zo@##SH!hlSkTv0O9j)jxY%%FZ6T?#@=)#88nf2!s*t zfI{s(S7grL6U*b@FM+|FT@aU{(B9rKUs>B$z#L6^cYo&4YR z#>-OxlFLYF33%m7Q|-UjNQM3swb07mt;4(G9`8B<%T0G2T*hq1vsWfhZKLxRu>&%o zW&Nb=60#^(5FgfNJrZhGd$4%X`g6>3k?j6TeI@wa7&a)}bk zDW}3y9x8ROG%?GA#SGt85LZfn)St`w@X7S{PfRDYt5Nec_Y_m)sFBeX(iJG zVIUIfYGI1cXD{>LNW9SoN23S3H6~tJA!-%rAp@P_8P7YsUBcdhc0ES=9YMM~o-g9C z>?q};*X}9$%lU}g$UJ7^Y;^QmG_p7Kt&+KR$YY%O)7btKYi`q!f$yRDm6{88RAsgX zMiyIpj#i55-+`Vw%N;l#mw^KEo3YG#?7kb=T>HD~dnkM%n7&Y9kSv|K2hAev{Vy5g zu#hzD>ReF$;2BDs(s25!eOBT_NmXlB_g_HRa;`AGcxG8z-ylFcpc3~SO-%AjW|Hrk z?N(Ux!Zm@1is2-Lky{U-(L}l{nAw~&3~e93Trwq-ye_>5*D<{8eB6LI)gK*=I)L(# zy(F;UJ^U{Is?Fe9fiieX6~I-j{DT^}{X6z)EV-WT3{ zudbI0Q~q`0_k_EJz=9CgyBefAtm^quS1?>dbS4%`~uuE}$E6CYe8?WZ+guMjfu zC52x;$Z`%+Cyr=$STtO#T?sSjjXz7sZBc<|)C06Ty~>J~g(a8BVe^7OyW(s3ZkDf< z32UxVdsCZLnty=H+v-#L8*T3lextIMGRfZO94R}&r+SQgCeM$9q4%%VM2{B8eioO0J~-YcW?U*PC#>ppn+Y!rYgdjUd&vA+R{q&T{;Eg7NEc zKsaG8QdkNuGuCj0zIK-LxhA^3?%EKJck$J+a1Fm~^{!}E;?DtKU%R0Ey>;uGR_6FF z+R8i>9Ch6Ks~{}f0SQwInin;WKEE>HOlSrqvldRFEpvyFoV%oP#rwxz96 zsf(Le_;RN8T4+?D;#X)1M9c6+en*Z<;|8;XY?|+f%d;G|G?3&OvIr(C86N`FEsH`g zE)}n`Y%-lU`8Cq2%io3~mCNaDYt`a%I`dvj$n9_cF%jSoRU{g6G#~}Z67O^qSuQWn zDQ6HjPdAjpsa`nnaZ`Cf@BWjnH@Qqpx%=^sH9FQ8eroH~eXMfwi!Wj-mtO7>`p}4t zkF&gNlwtAOcJVRv;Ma*Hd&@ZNS-pFK;l)2W!EkTz>e8KMr;UohzTXr4Nit(94yA*pSJjPT?A#-#gM`4|^8@ zn9huVE-$Q>^V_1`Ki0dc9Wtk47x*eP=IaXf2F@1^ z(j(6=6!bg43VwAqSDWc#&%P9fVz;NC`OoOu4(&-JwMMy~9~$copNUVj4XGb)?(-Dq zSfpn^Bm;99;f4z5Wp!WGkna5S-9LJ+r1+nYuGN93)6{a=mVGvd2Fbn!47rO_)R5l0 z>2syQ&WEx8>@;$GiPu5}b|TL)Oyy;cN+a(UuS8pCmP6aT4X|VP3_rAFOUV!I9#nqP zd0ZPNe(dftSQX17rjip^d#e}VA4cJraF8o|!RG^FWji`FHlJ%~y0L<+!xW>I$F&6& zwk!GsRl^WR4RdbbSe0UB7iMzok@fLs71H+7%F`y?ogeQK8nJZzLRAKcVN(27PrD!t z*MZo*QxNtZpRDASFo%iLY%*xGc#Yuv$L?OON^(<+PL|UMQcW|{{Y%bGwLo57?t3PG z%)=+*d{t2^q1=t`3Pbb6ZryVv*hFQeg<7BD2CO(l*)Ft2=P|vuuSTaq?NjJ8tuqWC zOt>h#>*$7Wr01s<9b!l0QbZH+)k8z9#D-?VuM=xIW0pJnG2?yVCrKz*4VN*cJ9sf; zH;2I+o7%5;EQGPuzO)T(p6mi4n}W>;AZ@MpPIb1;$u9^bDK85`zmZbiEc9@nFeu%W zQ^{A4;3=ElFP_nA_v^wf8%gHw*PgoSn}e0JtBVIkuS8}@HnDPNR-)Whnyolx&AU@8 zQYz_NEUH|!caG8a;Lva5ZZkqLOYZQ6zTm}GarCV=v2Amr@(0%52o+h?X<)`!iz%@W z&-nbRly?Wy`3ilJseE-#Xn_5GSRMC6PaH3()`>p7rUiGFrHkGWJA8s`*txy5!*k-R5-{+)# z4UO<KA`g;Qv6Ed#iP*mjr0o`9||L0ofxzE|ySS;1iuvU5% z6u571@NBYtC0X$2#pTrHKdc>EvI0DxQAg;+d?+R!{U(p{NQ>fcU`s5*mkuBJXD3)l zix`)<_{W8&)eMZ_8VK;ApISYEXqD#={$Dn4^t#x2=};@aBPSQ`y)rMa;TVu#JqpU; zq(5gpD1Pod5)qdK2boNT*Fvt;S_boM(4D*?vik9eGo6$K(rkkB)S>mmyUkBeN4vvL zH5dAds{|Tfv6ys!1hNP$fp$HwHb?kx%^rLFTM7i8IuKdyp+gs-Zss@Qd(G`Tmlr|^ zlEyOvMHaFuWyNq(dYg21C@tc9=H%?zmbExl%Dug?y(@NZYvq257HP}i{!|z82bTQi z$)9A13lN5vNss(6$=E6K(Me;Ku)NdMixHdESRK|&yA(5Yac>I-VT%+FAMxMT){fv>@|k8p-O{B=@UGYGQGT$f5Dy8>Q$`Jv#yU@2<*>10V;8ej5K zI0%H>+C;6su(~=7k5)=HeFuEN1dHcc|;9vHVPkQtlj%mhm4(<3dBcYS3AZ+#OZ2jB8^@0>n%QRQyHGt*sqmOfG-~SUudHCzZ zt%}TDwVEXYT9y-cyNO;nx#VSQ8^3@OFZj8SD(^upSFqs=RRtUCZ&bkzL3_7-TfQme zZu9Mqg*t4)-nQ30IoQAs8uN(oj%JzUHmxt#&pjt)SEcOy0ZZhxQEf^SRW^ifDP}ks zT$ih7VCD$m!d`eD=en5aJbC<=RAxWPu4kOY*|0?DtsVC-oc68AS|Dlml%KlfbkX%*cbwRd7n=$XIQgK>m$~kyeDrvO+WE5F>Vu%@hkoD& zi=vbxwC}J#Fa21)X>IhfC4m}Hsf?NCEM_k-%pO)JP6s>D`aP0>j!EOTos<-rPaVJ5 zcT|o_>xgVR6Es@2o9ar7-_keFg5TutS|gkCLk3yG61sBsK?aZp$$E$XcgVUu$gj!!o{RKc84}Jao@Znb(`xk7Bm1I})ktYPQP5 z`3iDbrUL_dseyNsB>}=E9rvVLmI6<4&yWda zSv7rHrGfbhedye}@bQ8GAmtjRm9-S?6xN2hz zKD0~f9lssvOSm_kE3l0Fxb-HTB{I>Ejpw4q6QT#2uhp}$GrG0TkQoz?RD1xqkY@=W zQOzg_3z*8Sb+f7_@7TWNLYn^gDkT};+I*?H`mI)gHTYn#AMR=|{Cp`mZ=s+hl8R4( zE7)y%WfzcdboBeJg(Vw;Rb9KY1##M#Fs*yLAR{4A#}cNesLi}PmA!pcT1HO&3TlbVN%slI3pQh_&Zm@kyfZw`qXb* zNTU&P5aHO1tuD$!m%*d|+C=J%zC(jAG^MqGzh_$@_t&!!5_ zC;LruwWqOMwuBJIa9M<_5Nys3L2sWlBKXx;=^vPqg5vPsBEP zuw(}N9qNzacB4B{&iUKgk8(A>)!eC=H6GCW*+y5MU~Zyvk4vYcAgl+>MJO5J7P0L% z+0|MVW`Z#3y}*_ek2hZ%TS6TI^}8QTOCqSko^snyCDbC}sq=C;$MRc-u3i7ea{=c{US?&&u>&wQE43fy{a-Q@bRB+RsJU(`}%Ps;!v`MChd z&gLyQSWmLerBpVN@kefdZ9DkR#b^!k>?fApPI`_K?I|f+tT3GA;9_S7;GQ^7-`0PR zSG0q{l5lCl+F}TzXJGtv7Dm@iY4}gh43FV{6|$K6jU2~)$x$mY-8+ZvB4um?=;Zum z9Bk@lE9;EHgx2eO+YDGGfw#C$xw6)H6iy z38P;OJBTMB_FtP;=eGWPT<|QXIDLh6re#d@`242-{J{F{RBYc)#RrP|(;Bmc7i@2J zL?@(fc-%1=N~+e;pEZ_W;5iI3&rXhM>T+j~J8Dy!4ad6n6CJBc3%@rNXzA4%{>JXA zyzzbXx5*NJMtFE%6#U8KjX{R&L@^v2LB?s%@>-Cq4`Aq~rQLpALR>$|g8$h%aO7mM zPZ?B;ig3uvV@>%!w$`srukOz3zZA9e>x3u%tVL9i{xju!pIj|Y=5i1J+x!?%`(#w% z&{YOQcXeyT2HUD3OB?s(a{QOGTpziEj+egyc2A)vhzRBS-M~{RM}zRWMS%TBC<%++ zU**g}YsksurWM&%asUbMG9|#dwEXS*k{s`T3y)|aWUIOqpym(chy~jLP#`GTgm2R& znA~Wmr~LzFYU*pbP(+UF*%y8~S=~qg66Tw8FI)H)PA#8pdRE71+{HswQ?cQN9f84~ zpjT_!R~2|m{671#%Op^(0=<)wgPalio#odPZ)wESart$kVrzeM2T#Ywc-4#yD=zXL zlxbxKCJ+Ps>LQMFJ|iW>B7elhjvPX26R(^{;1vCH2O_8^^eOdsJw1J&Mu=FKI- zOSez$RsQ_pwWRcB3tRi6N=R^wK#8vSF;2VQu3bH8fTfvSA+5k1-U8N!s>uSY1z0{1 z{@qyvc4!&#I-UybjpJE_e_+7&GV*((3h(Ki^4w01P|r;-y5r1RQ}Do7y1F9Hd+MF_ z7`wC!#?+^sU$5>I?Zxglj{D&5P)JV9It7#~(bW8Vr1f!K)s*k^j-nDKfVS~Wo`y*k zeqP3UxqC_k^*71B+fzJ(H)m^$*OG2!TTHqPY_z;U92tg(27#NbZQ;oWo?$E1hWmkz z6yqO_)|a3vwti?j-1`_2UlKu_PE>=yy#Op|TyGTT%}iE5&}63CY>H&f?AE{e2?@axUbL<${g6Fu zp&RqMYeq>x`aENa>>`rBt_*F>I%&n50{ZP3T5+77^h|uGxu#+WbMk<8Xkwkg5GEm~ z8{e#ooSAdT^&S{HX0lglw#AB#!UK8lQ>V7#b7LdyJL|}qDchWfXauzA3}EgLslfa? zA=D8lAUC%fSj)KsT=qcG5?liRQfPhssAkKm@f6DrLs{Pof@7c?2v(ur=HyfdJdiY( zV)u*~J5~syHj(448x{?&ZAfK*s-5ZX@4G$~c|f;bv<-{>dgw~zRos&cd9v@Gna{VM z2s51KHx{%h(ssoy4kq~~#Kd>^`I>CuNL?Dy_mZWCBk_y>(Tbj+yhH6Fg%Dlvu)68+ zYMqWCF$wn$TGTCTnIAZ~;?(oQ?Yw!p;FDEPq6)AF`HCMn7CD7kL1AFfV!iX?)0Z*{ zLcNqI;u)1A^FY0b(xxwU?|?>~u84VlThwY!UTu;Xf_Y){jO!|Ay;fH@-dbtxR1ULq zL?WAp4?}(g9|E{>pBLOX(P(=;hca;S89()^b(PpeV&--G%5*KXjNVlGU@-*#$Xq`GK4Ee4pZ5becOd9ruyT;9SC zB~pGyOG9@lV#aZ-V|MKU)^HJmw)CP63R5XhzRvn4+4bVYr}35Lq*z!ZCRDe%YyOip zZ@F?M=bEL3H6}O^gug|<5g)L5Fp?lCo{~zz( z#=T``Wol{ZEv?*Eik9=1si~=%;sBM@Tp*Hr%Wb$QS@ zrEOv7{n4d%i9by9mui7-c(A;xyuyEo4zuZXAY@Rldc>KUzWczX$+xar;P6DVaSKnZ zdh=Mg|ouXmlf@A}A$71|WL0GN&I84l&rQiNSML%t| zM#05bZV({)O^*f@USpqBKo~fj1@O?$iQhB+mH{U z&xjDEcVL3e?d{!VO|swdPdvwY!|J=BUpi;J3Y#{((!90WNXgfP0p|!mk2!={46*Zh z>abg*9HujGj5=@Eogui-Go0Y)o(4fTbz<{3)@~d7(q$h7@YYq)SsN8y&WR+ayTgGn zMUX^y_Ll4pu=x*iE#I0LyWm#7=7Uk#!K72NYW4J&=_iC@!{pJO>%i$5m63}ocfE8L7Wh1Q!Tv1_ZV zmbx{>-s{RdMu?Mj^Vmv6tufo8r}h&3 z>(FkjKa%Ej@j8cW85wwLm5V4O*MFL}`9n_SwNJ2MFtRRmED@l}QeeWiw%Un-D52Wr z;`(~@oK9=bZo7>A_>~vQDvF3zLBzQgtm|vz`W$($?U9P(dcK8ICVVbuxL%VEQG5UE zqsaDm5+l9<$E6fpN1EkSsOX$s6*N-vi>e}7unJO4(XtCoCL3gvwGD%27_&3(z;)Xg zwjM@(?VletW#m;Mo9Z#Hz&{0kv=Wx+zcCv0eA^}AVd*yiG|oV`A?cvC_YYgk{Reu? zI9Bq-pv$Wh;Pxgr=eoJN$?O_UWELG~G;yA8x^xBNbyA~3I&RHD*%|s)HjV-``CQ_x2pV@(nz ztJcCOM_hYRp1_RUmmw8wGW`2+nEFAOwq4pjlLj=&;L`*k=m`-Hb*NCxrf>Ul0qu{v zEUVCJJZao!)AKVyA`#K|DOSOSv^_4=q4|vmv@F7j+9PV=D=;lBcckLWVAUk(;|=)` zgfcnbKZL{b`uIn3cQx;t`(|H$=RMw#B`JIe?V7%A!~;z*g7xYVh5q+15b&L67;*~1=q`Ir&NiNx*#;^|`Xq~=kNzJmzW{S>I z7BN$|;?mEQq$sDTPiO3(J2*7kO4Fu~4JRFH{L^#>_g$512 z6-W~^qF1RU(!m_FCt-4GqUa|mf)Vv%xL_bK{AsF;k_rb=>cmNu z4|aT7p52TOy+(>V##{5)`abqLK=z&@NkP%;TS(n_bq`O<^ZRZW^LZUH?&MYLSJH#d z$X2G;E@*_SGkSuUXOt;&Zrbc}Z=p9Z*pK4WN0W=$FNL)pbc?k)%sW=S&ohS1&zzwbHsy2x*UQIUpw&;wx>k zX;34{eyBUA4|>>4$P?7d!`bnz%#}YSI9j!_}gPw*Hp%m=$|I&YDKR zo^Ey&`w=dmU5}@%+m>OYnIL{&u*og63j#g?>mMC;e(KjRnt~<2_5bL*zn#CB#aebzrnQmyt?nSY!Lw{yPHfFAEPbzOK;Il@Q(LrM|Go?0EfUN|0 z!qq_-gylfpS1MRW=4NOPa2pwOjE!EIHEtm_N2%39V0xWWv|1M^{tHmPZLJq>EbQN1 zc4)rmd)>Da^CByDi0$?y&4jAp<%FUY!~@IQu|Wv`LP&Z;&sENhT6tKXS8Id;jo`6y z=F_mp27i0j<+1XpjKly<&u2D2a!*m|+Ug{Z7PYze`F@Gjvu^M069iq? zl+1ge-k^!Tg+Z5h8PgIhpv_T{Q|sHAr*8cgjxj0cK1X6e@K-dau2gM!JKL-j?H**h zYMS0NWz+k^bv=50w8Y}{8rC0D8YZQnh=P(nh+;Nj6BcIr$ZO`*!I5>sv}Z;1cc z!$n!4EiN#>)PC<%yx{nROcM|LB`M!N*+BqN;9Mk^9IbLIMrYvqJ!u8^u|sa710XlK;tt{ zHmzgIxrTH051=d$q3ItM;>NDLrGwlw6^NB|G0#7!0oRf-zme}V$CHn>(@OapR&0{)1M93=U zOHX(PwTb8)rDzqGVshg()D|llWRcq%)L9yB^ z*9yZLIah;BO`I*MP0pd)8sFIaTa!IrgfdBcoUOi@Rp0Y+A;GxK@I-pr97CiX5^Pf; ze+5k4KZ9*6D%uQeS5SsFHIj$d-ehhet>^y(Bk*;Jif=prp_OwLY|%dsBCwJd%dgqf zs14ca<)!_3`MI$_yA+vMx0Rg#?mHZe8>DQ@qcRveQAFg3?2FsWb|Oid;9%#{`yR*1 zoY2_wN}y!@f~zIq<7ARKYGdRR^ zFYvU}+jecWu;OxGR*qC%HOc#i>Y~lJ>6Le&06vzw4v?dKC-lP~whsEJ_fCxpdCMbr zr%4q2D@x;?3ax%(6UzrP^tG*XTgh-f%j5q324&!ao&9 z=l_bqwgSiuu5k(KM@09xkFzAv((p~3=~Rn`MrrV@cOrEOE!d=25%&E1aLa^vny9f# z_j;Ad_67cJ%dKucaLGIqG=7By`04~_DQ`kf`-G81V)yDZ5Az+$Y9&!dy@LvF)t{?t zgWgEEy}Y(Hvg9RP3&sw8l$0Je>JTct)Ynbdugsg8wrL6obnOmUmd(q!(5KW&%BTj! zFBT;A^X$z(fUfK~8tirnN*Og(vgqSc_qNQZ^IcH`TuBmup$86X9@=NVsm=VQRjvu_lQ)jft(01q@1NM+ari|oAe?Gm~^r=teaiyTj@#Q`gu8?>9zfrow&V2FD@ z7i62s!zes0G?(*nJ;C$|s5CQ5-1)KKnPRS5`Im(+V8TUb01lIx@sZBfNPmUL`1C$)<9@0hy&j##za(UpEsn@@Y)H#dQ-kK%V_q2xCI*U*5~xZu=HZ zw=`>nXkQ)uG^+SXn04Duk41XuLyQFEsIwj!uBAUV_4<_A z5`J9$bN(A2AL4*zb!DQnc^n3DVSMN_AYtT|JzK5i?6q5Vf01gNJ#sVDRtj;0twW_3jRX@b!ulN{A8%i% zJe$!!Be{%29q;llcW~%(xqN={KbP!}>e$Qxx$iYvidw22eD3pqrvMq#b<;|7YSBee zG7@$qu%DGiyf>t4Q@7=NYtw4&THW+k6sY%eT{J;XYb@>)Z-_HE*?G|c-r|^1=g%&m zRZTP;JHcGXD|%SWjYWRg09kA$&d8sE^NM4d_wPHE?4)uAXRamItjoPgo?D74Z~&dW zf*vP@q-e_rMVNG_O>3a5;XAzISl*KZs^Sv-2RxDI!mvv~pRb9J>_vOJ{60dkF438F z^%&9-o=9VO3UlO66~1nxO%)u%Gkug|#3kxCRVM7Kf{jrZ5||n{98`J27GRzj8*TG_ z7wN{a8XCIpJ|88P<)OCE=4pzjYhusirL1HM5oczmfn!}|$9hdxenQv>tZ&`6-qqB# zawk+gn4;use3`W7jL5&l#h_yU$R5;t3S2EQj>i5DO==$FBo48zERtnCScb<7(btvo{kaGGG@hC@nQ$DVf zcedUdWp;-V3|y|&5G+KX!eWiL#4~^vKG+Ko4%m8hWj@k;T>nDt42BT=dbBQxdw;H8 zFnA*mc=iJs=v&3Am2lL@Yyo_M0ApvHxwzVV={C*e+7jfqebuLe0TE){e%oOG@WinMAc<=R>@-7DM}o9 zNQox;Ea};lyAD$t-fd&zUqar>k4axIrL=PUEB2m!8wb&~RW=l`aiAE2lH&CY+Qq$i zyIHlU>BhLR`pV6P?#cBXORq^sSG?Gv$edI%r`b$5_BPiFkH{!GBjW+SqO$25-%tcd zwA<3|WIoacqrByMkJ;Xb2`7969sMTcROy`3(Pi3=&!LFVIzKYmDS##F%D@}mwMR2e zn&ztOtDI*k52NNiR;$aR$g zlqR=kO@Hidlu+g!a*LNMl|mMBH(<$=fGy!oZQuXDKS1IH`>COgstDsSAe=@6E~x7pF#HJ=U`1wDl;CW8z*PW92*V}0|< zJJ*G%mxVNz&C#%H;@8aFs|RzR;IOj=BQ=xZ2(z5-btP5Wtg0q*8j)1JL5NW$OlX=b z*Xl$G3FS+?@pmB1#TR?cG@9EFwmgN7z+!t>N6^p4K5v+&!2O1W-n>8qIN(n5;P$CC zB*ZjIm5Uq5-AF3Jw{wkyxBVxCi(n^cs!Zi}(JVRi`G%;9WZZYsCR$krTpqe5o=|=> zms@5(z$eS2n!_8<$h`|E%#%-Ty{SDJKsO8Wl`5>H$b^1?M9?iMwV z&NNx`IFRicw)1QqRFW}CS>62w0Ft1y0pUzk!2BU@ta$AOrJu)SU)uw&h-vlP zJorXo&QU#xf7dPyoLAyJRe0gCL1{53bvdds_bO=FK%Y9LCFSt&j{bb&n1K1(=UR#e zK!J`1g>rS0xkt;>tZMfu!vF@1xkbH4jMuk2toq;7?9{q3){;mPeKJJ>2n3y6*%^WL zuAzAp>qBYiWvNU+;~{*22fbnB9^b1kl8azj)|jyhmTiWDvK~g-_gA~(cq6;j-WyB* zM&441M^YF!5Q_6($zuDIITOdb)6z8JKsqYkM<7t>MWk# zM%(^)**8a}(UBth2? zWd{TLWG4HjkoTr?YqhGNZ8e(p9508B%Mqo0^Uv}J#w%~azp#^R@ATH?0QZ^J+T$fz zEg{Ebi{!-YSBF`Ac2$LMfR)A3pr(fbh;=kQc$jOsQA_Wy7W6&rpr`7@I|y3MTklg= zUynsJC}xX!d!QP5bpY~2&|0Aesy}@V0`-E;Z2|q>v3RN`Xbl)41-4Xt+Wh}_Fz*R; zJL*(lO=sDonZ1?WL1@^&-^b`8KI*{7;2RyeJ&FABT@UzXMjQC$j7b=^aZ!N4APmra z|DLo<E1C%$_Q~PS^npzMMbQ=JB5hj}5_v z|5NS%O^lqrtZ(Da6j)uq7cAE``LQo~4ovA5d_00ZER4#TId?3psayaYVBWYGMHr`r zmwqm$7uOc-t+&cIhOfFn`Go=nI(dI&<=Z#jqkQQ>#RxKhkM{TI=bj{VKn;^^R`DF2 z);f_&U_p_{))K9DT(=a8>5r!{I^7)b^_-D#r#JS^nR8t>Aq1WS(0LXG7++mVJl(`a z$*4Ws5nwWtr%HL=jAR1e!G+jutHSo#D5}dqcV{;Bw0dspqSITU5VUP>XefwP8SYSR z4VIXSi=RWn;PWl4o8A*@kMrFG>D`sny#R2 z3qxfQn-bKrt!?!rT1;2CZ=0Z^{>w!r7RJa=ofI1|&ruu?L<^eO6MhV47Fst94~$O_ zv-MYsV~U3`H|_=-n+ACIh0k?eii3L00&}c_RDo^Nvnhc!V6b(4Ko#L$KMCsiCS@BQ z&mN|lLT*9Q}cgy&NcgZXXyybI3eT@vw=0sdOI zwR8q2vH_I5g3)qQtd;*?*~LD?T#q?E)am9%nw|VfodZe;3>rYW#(DFZ%u-V3j2m-w z+8e7tJ=)k-#8C1YMWk99EjWpUWi1lh-Pxf}O$>z}rumt1!LjA+iB`kSzxM1h|6(Mv zBrTn>9c4CMQkXc-+fjbiG2mDcT7R{j*XMAJx@V@7j!(c>eyOKji`CTinOV<^qZBe5 zDuX3%S4BN5w-J}vOeZ&^6`DV1^}XS)O3A+=$KIn<^ex&pXz2)T2}dvMG;z?Zjc=OV6;5lfg#gPyKSVBl}# z9lFA$GXh-AKL_j!Y>b(xs*Wk)^c_cpVGR|kqh1i3Uj?ZliA|BB?cb#kjTIy9-nFCN ztzqkeI~6mBP9+1hRXYI1G>#h3NKbUU6r6pamC_gXbfYQa2HU@UqTrkg8HSO1>+<>( zJ26)os(#@TEQ9h8nQNS^I^ozLb1o%w475?)rX)a#4D-b=_iL~9enm$k!}eUvVD1A2 z-7EHYCB?psMVnD?MS#(ns}9wdv)*1Fi-S(_Y2AOm6g}-gar9FxpL>fwJ8f4do8(c9 z)9`+~h{^PIxRii&z-|z1rnbKcFet(OVSD1czI-E6FduGOL^SC<`&!GWen+n6Z>1|IsN5Ko3>hAQ zriTNB$icF33xjyeN&0awndQ8qOfRfky8Zib>cw_TxKEYcy_9xCe_zC0;jm;v?Z9p# z_MJ6M1^tWd-MCpixh!)!DzFoP65XD(ctlaK%v5gpyTZDDTOxt``v@=w3CV+kTJ=P- z>Fc~Jk+>DHF2Se6&nWR?SA9jN_i~HP&BCHRoQ z)^L7JouBrKgGXR#!IPbkUwf>+^ZHl8NwTesn|RtISVIw7Q+kB}GMx{*xZ9mbFWL^e0Kll}lxWvIvjaae7kQIs}UJu1|U1JC}s?;Q7hiyO@rJn&y zN(B1r&Q9L;>u1I~1x|CmV*2aHq?&3QKK?n>xMo89{wqt~^GW@FB@1Eeu&0Of`Q=R! z)Rc4aR?SAa6t)e3T8M3{b<)Y~%vEb507*7Mj{d874XmkOb*p?SifLIFtVx*Oo5O`C z3Kb^(M7oHTWn_3+MLe(~Vn!C90SkXMgR!4A^Iw3O}?Y zZ;9sjIQigYeZcqa>n9C6Chf*!$q-zrim$%YTVVq+Q<`WItlYpmYSnHthh*?D-)}v5 zHB52?=has}jBXCuJ&ipXCc_OFQ{e4Rj@gIxmrcq=*KWEjF2%SECw=gX!KzJs(AKq= zu&>B|_o3%9zs!v_fsM=-c%5({-|r(R)MH@LYcQ)Y&JpV59dGtn4a^j-Rg!;UtZ{99 z@?wwZzy_^87%$hQklv2D+A-cCS`)PPz{5U%#nR3$RWfZ>;>|6}FY8m|)4z{Mpo}Od z!>*2TwC!hhcXkEhcNqq{BtxW1W41x?WbF@^NY7d(~?J@Ae&%|-Ky5b|^l+j|IY z6+b^pJVZe2YMVC>>6wVXT{(~E{{}jW#&TOFy1wPzoqCwP|3T@bk=7X)PbfJO*jId0 z!XB(p1;f-fYC^pWJMiieo4AS~o`YPYO&@RU4B~rgByny(XxSQ{JUVPgevN01F1f+lhxg@M4~} zO~nKZ(gvM#>Et)0EemITjkuf)%-u-bun!oT{5RW6TR+*Ykyk%TE_4eR#s6cnT{0!2 zg~%Fa>~dnSi{@nS9fQ?3Jf;^F-8_ec%Bsj*54BlhDtM}78>aS8XarsGnmpSw9Hu-w zEn(GE*Vs7Bs3@E!g;g%%{^}fDnLx)l)z!xIcr6ph*t*uv0BCQwhLmdg_tuq3U)VWa zftVx@P2KUFLxbaQ9<9i8m1o=S_$AEtDa)@$UjEkUP0uTubpTnh1 z$=Ph;SX<(WEh&2?<3@h|w#v>oiM3ojb!WOg{?1$-Od?2|WjExdz!1F=cv+qhi7$p! zkH2qH5Tz~Z<@SZi5UXK)wqNC~NP;a}H!+oj{zkGD;UOvNtF^@Q+I{qLoVCP8=PWOO z)?OQm*jM-fW*IDKBXX%VTIdkt0<;@>e!F}pK_}f(ZWIw<_jxB@?s_ii)VM$AI)!^e zsMxC-w(ofA_mN3ZiOQkkoERu+&IU6tq4UnCkur`S`OY;IFZeR$VN$^4DbQZv(jleO zs&@yA%Tww|XJr>UMGiTT5C^@+H!$`og{R~pFE7bSr0;p{yB;U3%9hm?tNB~C3Bk^C z&XLvne(Z`)YX;@!Pz9 zgz2`4hmqz@05)tNe))(q5B?8y@F+8`}4vd$$i#Qwoo}#TEjEFOAJHhvT4s_ z^7p}77A+faYjJlfNrl|>kz4@I1EY5;9=qw zs{-kYPNi_Xl+x!qs{^Vb$iN2jL`?xQs;NACXtXrYNW3$!#oO+6H2vT?Dy(~UtCHml zWHxPshATh#hHH8kBxRW&fWSOjCUUkpPU!#PK?7 zoIEUo%`VdVeMEI-EjOw~*!-B|Gl#h5vNR8MsX&;!iT1dc8gn}!jQVl8(F-Xe63%zv zU-#Tvl6k6e@Ak+k|BqK|Q&{j71?0u8w#BNuD50QN0%sVhbTiqT8b=+I`z+YbPwjms z8Mn@VSJUrMw%5+RUK1MjO)?GD$7!saEJQ3S_IT)fd~_`aR*Gw z6<(*UFHMLzOJEv)aK}~l-T@iIRH=zJBlhjKJ{H-1ga{#ykZxuHxwaKCEi#SOGHO2- z{RkqqD`RrUYpOI4=iPreHSj;!(h&2{nA49_Tr24)H!K&L2>TQ_(^gq>=&^;6aElL? zL~+0l{i^V}j6{_ZyO=K>C6s%@<_sLMCttf#$Mvf0(0K3 z-`Q0xJTn3PMb9R!O~CnUn$vt>ueZgwdOv5FM2k-Ama)HUYL}Q^Gr0EKt(IDAIzCa( zMl^3fjzguy-zyK|fy+f2T<((H{M=TJWhd1#F9pKh%d#rWjL1zR6-HOIx2P64Gw};R zS_y*GPU8Z8@Kfmv(2YCBBYR&MF~OlXBLcr#SI(pK8mDs;7*cE6}Ys>#GmWpKiP_ z-G%$ng=QEk=Dk!2egDt?hxJ1LaUQ6+Okqtu)Yt>=e`&a|>RZVG8`D|0S=gjD^E@?Z zG>ozJ;eRNAh>?hnp#`N&`=tgcMHUIvk14vkS(Z&HiGa|8`+YCUZaLF@vv(m9$D)V^${2Vx*|=q3y`M6X>P^ z;@()PVUz>4H3-)ZK}XXcQbw*;d7Sw%YBfo&DyeS}wV*uIJ;cgR-a=suv`0dZ@u=?Qc2Vr~Ob3WI@d`XcY zwFIIPTYsmf$uH$6b5T;mtBib}igk!o9K8N0ix-kF8KI9$(IYx#FPg3m)NWK%pDC5! zT0Lj{j9CW_zU$2%Lw{_Lr}U4|GN3Y26goh2(J|0`#5jQkV)#efWGz z4f)nF(o)p4>)g{jsU~ZFWQS3_gwQXaRjbv@)=^(GdC-mz@EKceCJ1I_o!SADRn5>7 zd(}y6fIhXll^_c5I1`;$l#w6Ku{Wj}o=~&cMe(=e%;<(?cH{ZtPX8$m?~iHCX7!;h z>oXY#S7`5IIJ#yvOd!uLZhW(NuRZ6@n5cI84MGS-nwYqEo9m$gwS>tx7bVaDYvtPD zr=$m)wa^cpDe==1HPJt*%0zNhBmBwB?$7h(8mr42hIbB)O+sZJQ{2#Vi5v5qcm@EKz7emi zDM&K~nqm|b-Dp(bheJ%pPs^s4srr`*cGhD41?+wu;b3RO*d14$&F!VIne_9`JBlik z4fQ{Q(puF;;uXq=EngWv6|C2+$EZrg;4lx>tvSw9q=lF;-|kBOTUL zGb`IeT~nTcqE=b;@@op^)||>4vW$_@z|m5L#q8!e{mB%!;sa-0u$5To7fqzUQF^~l5vU>pM? z4R*VwOOaR=1x?#orMMN_O!a8z&O`b2-mjml7_r1TNRMdVn=|3x*_?Zi7h|4eWCzBD z4V#aQt@!#F>P-%2gOXz++s6N;q}$bvHDx}(roxllk|tq2yi$ zjEt6j-QM>zS=4TsC6XgasSX7rbgf&mQ{sy>ZX z^mRbG%@3UN*epo~r=Pc zt?g-|XrkWK(%P=>wGwaQidOK0Jkuk~*4mv{=Ez5^joL;l#A*FOE)we8md~7!E4Qny zZ+>p#=;i}92(tHKd6Vj%_KjixZ&4!+CaG$;Q+v!4G_m;Dr@D>VEtM1x0bnE8qPNj7 z=l4fprGI9Cwx{(O?M#r2o>vl=U(d9ve$%Q@d}&+RA^K;wE<^OOt;Pp!i@spiL60Y; zo7%jwx7yQG_p)oh10X<#)t)yM0%-$L!rhsOU4!|W|l%MjHTky_c*}j^|5BCXZ zE25^Cy+Fi9M4@(Rep}00_1e;lWqZ-btW%|Xm0P=WP1Uphd#gn?O1vNR<}-8DwcTSc zPK-&kAZP{ovL0Av>#3}Gd6U_9WH(=5fIn6XFe81TrkQ15B2ToZdB7m+&ZKm6yYLly zmNS3EtGUH$;T1FV|W{pFCbA{|I+Dk=#m} znGiLK6{DL!w(0d8#5P)sxRmm_R-p80PBIsiuQVO5;EMa|Uzk7w_Dk)Ho%~H(ZOL9W zeIs;OCP=h`S_%{=6Hvk)r8sw%u-8$k!75;cfj3F~w5kCRhz_d;X*@0ef4YNy&5@Hn z1nL8F4iToz^%$CIAuCpmA_Oh#ceA}}Yk-~wEN14UBVeOb z0V(Eb=2dRh<@`A{q2DQLpsw$WD;nODCQ}|gCUEQ%YdM-7A2is_mnf+pfGPy69etXs z*mEHC+3rPZ_Vgv(Fi#FX^Mm&Xb?GKTGvaAzWrgog@#MqIpRRSs*6Y*QxF99}&(JC6 zj7GJop4M_>(n=Z|aV1dNp0=YF;vCk8F&T*@JRlx+j?V_l-<4>$4Y739o56nQH!&_J zW!L@_vAKk4%%5cV7Y;wo7;_D57ZASrU&N(V^^ONUmrqw#1E$I@bd^%{0##G5oShWt z8qDWdnJymzXX>DIt{4`xQN|mEczpGYA-xx{`?T_?e&}j!l=B9&75eYBarsHR>K7fk zlyH6e$oeOGNYlOXK2^kP(G4|_HH92+7Fn3{Z^}PYeKw=KJHr)(43)PP>j|w)tB+Oi zy?e|b;~xqpQ{AH+NhjX3#C_eaW_(J0hWp6K`(iOtH0>>8mi^=fg<;9!JH*baKFG1! z1nQYz%8p0(W2A1<1E)iH_aA2~qj+N7Hn`YP}cQG>$Dk-2cjK$VTV_*oIK*!0?S9a}8JE+YOF2h-4 z>gn|K^G(8wd1*c_y4HvaeNDA|n!`Q*D%Iy!_?b8AC!@b6p!7M3R?9`&&-Nc+O16g6 z0_uh7$o%da6NL3xMfr~0n{#^pFUs$fIfnp7_OB0b<2`ugf0l6NI>J;35ik*uM-WFb zBj>T)#r;5ZNVCuA{KD$?(}Z`0GH0q;mL{YTNn&I{#=kMmw#nVHQ)-5H#uC=o3(l{n2xOerX>*ECP+YBg;HC3Pd9r&7)t+avp}>YD7csg^CUEUo;2b zBZi+DEZPsDLSvg(6-~??^@Ang(o>Ln!w7{SXL|o}6H5 z`hQ)Sz|8pizr<6gE{)r<%QS@BQt>e6Wy@u}Si6VXI;^=7K(jWP{>3nYnMvWd zrjJUIl6&6*7Bu}P8iEt5-1y{ID}E>WX-eL!1q~%|;tGO!?gobJxpmBR>vo4+qaP}{s>IAY&Gjj;ady1D z?v};}!m0eT2yERZ5km`4W27i_PH&O8GwILKEkji9b$8>Hq&vFvYK|1UQ zAXf^kUMFQ6>`vW(JO5t=S(+!k!`EldU^ERyKstxGj;IQ|CSlgO{$&;wb{D_j!qa33ZzQrHA=-?o0B#!184noE zy)WUHUBdZnE^VzM4)VJra#RaLv(LBmKQSLRQxO$tQ&6^|LLa|= z63ZN>-;&wKHIbaF)9gTnd!;Dvy)0C;Xqe#Aq0f{wnzK@q*+!GM{I+4=K;l3WA*_%~ zO~+{cIw9&~)3w0Jaxo}RfWISkx@Y0xk{6xdf)LXLyM}+=GV0MejaVD87R`(!Ty3L? z4rUE{d65Xb*ZRr4i^fW&BKS;N^e^=)1Ry^_yXgP^K62*{3x5SM2abv_E5drehyd;m z15srnqMQ{E_7+dc{Zgj7k5E2-p-X4>cAaa=&cJE8qpfQ4Xi<-j$sgLE_Q}$R^!#bV zE2Q_z!~VVp0?G1Ry;5U04;6xDC<&+El@N~5E73t?;%Kv>Viwn6*R_Ke}F$#&mk=Hxz-OeG2Y zes=RUQlP}Wiwh<9cYOTJCIpHuz6#k7u<7IP;3Pf+ZAxLPuD6f!z#7ueQUz2%)dO zwoFwZHUWoM$u!viMmjQkKC#f{vh3l!jZzUi&jqklocv%iGhGRB`bCy<-RJuH4`v)%1y(E@sIb`VS;~2M13g!ZI2OIDppYxZ9Uors=Upix1jViVK4M%8jj3s57^K4# zsdr|W77O3~Yr%$^s8R~Y^GW?ZFK;!nTmxnzj`h(u+;hBEl=` z2NE0WFqsrEt>9iLQkNs18VE)owO7g7S4cN$DqGWD@1}byZ~^c8$v-C@f{^7qR0Ur4 zlAEkHQ^=Qh7V3(^wyAHUCuuSOhVIq!d!sLNsE5R{7WjRn-Lk%TM4J#9Pf-TkH|gB_`Rt;3@U}{`!2=VG z#8v+nWE2M)0Uv2fl4d|8OD6Q>ZqhtPDpvAl1h~@p_k)Z*!bWvvp9b&j%xNW)^F4>7 zm++t2-=NW|cTgd#8s}vYR=WGyajc;3^+A!7_T?1b)_IFf)dvbO2+wbIP53BBg z^2kCP6Pt&z#lRx4;$E>@G*`2%Zm?z%47679nSg#IJ-!xb*Qt@WVr514#9(9nf)|}z zCde1SCLvpVJ?&VdO>*Y5x=E?02K3t%U;VQALp(%70O|R7sa{f2c}lTOKDzqU_r-I> zsAh)J>Vj{Ew$%4>B)60}iHNlf+tR-%gSXYZk33UQWjK%UAAYwk?EPc0k~cgSN-~{l zT^n8H!d6DHyf@!kyGeljj(C`@itoF&(KW6AKL^&IV{_J#3S*9=+MZs3^bv?N-ZXBp z*9Z2I3>;>{Pyu}FS|pl*8HxA*v*NFJlr=o!_mQ?yZR$;@`WLm0uQ_A;;^;Z1aFA}h zl^pVrQfl+KqMQyH*Pf$H7X`Vs$T6vB;lHbd1~6tC}MLKr(blbO5(Ypf&sF^?ix~z z+yYdciT1jPuErZMOx9tuXvMK&qw`*q+RVDe)JwoSm^>=X>wP)-IV$=O8A)o4uejv-JyVco+SwjPnm z*&qh;&R=a5M@0B3#ZZ=U5&jZ3@zY5=I!j}7y>xb@O4sXI1&?$x?EOC#A^oG>oi^e6 zJzqq-r|#}AWUR{V3s-z?`d2+Tdb=*&{=Li0W=9RB;r9w~ky`PoKz(x}88;x0g*N+wGqgEgG0 z4tf=EDi&rud!@FX;n^|2(Hvu?KivZa>-H!35|nwM!o$k>M`K4@F(}?&PtjidQaY0dDwe?0euH_93z?6W0_+nI_~q;nsM)fK75yxJyX z$G|m?!46aL>km^V*aTm6+l(17wE!F~V;$i~Yte10Wt81z6WF;kjVm`cZ%y}%ghz-} z%iN<)m4>BUCuh$62m^WnPCiG}9a4$Q#)rvs_T$=0^8YKyipM_zM%G7{s>A^i{p8KQ zo)P0)FX3R6U*=T2aUMHIuZP|L5VeOC#sWSwW=!BBFprTAv%o?Yyz>e@EWi%=CZ5W& zLQh;6&ntx%k{9fO|Ddg0P|EFrX|)mC`mZ84rq{6Owu780^qH@gS#4sJq!0f)6~`|; zg3rm`h&4Lnqofw;mi9r@3=fPUu6h@4zHT*$)if+u!-eP5@p}M~RbxeUQw30F1G1yaEEcG~3Eggb#PKB;(!FaV z9hwQfp2e?rh*Mj34n{K!dl28xY=J;yEL&(8^A#nip*B-8+142dDf|E{y-dYUxcy7x z10LNTP9xw?Aw}gj@y!NnyL|>6Q6aOdAAt|~wmSUx5lqzP)m@O%?ZqE(FgiQ??+u;E ztHAy4f>YyI+$%u+#fH_UpvD4wZN~2Mqym=0L7-(Ro_aFl!og90*rb~!O=M%{@K5=J zbo z0`hAz;6}D9jzzYys+UVNAFD58;B{EPc!L2|lBp=hl?KDYmfok`RO{Lw)^Rx+t9K{0 zA3`ZpkZtLIIlL5D49cIBH9%v*~2u@wqYaAX1 zUWO%@jy+VjhE$Jawi&MKvSYOneIiz9=5P`oT3>22ewo&y)~I#L?A*0&3_&e6iYE6G z39PIrPp04`yKPn@%x%cySRPNjm`Q_K9f3IklwQhB(=9NzRMToqZ}pC zZol^d9)D`VYpxl(+{?NHkxbm^0*~k6?z^8Atqa>`& zm{9*|jybN+V<1=0i!u1G|Cc$14IM0lEBax*zi#y@Z*we(>JH^4KXkf3_b+dUg7gZf z`LwKVTJQVTl6W%iKJH|N25SkGWMJ{!Y!_PH`Ok|8W*gT zszpn2T1xE}p`|z-c2S$eIIY%5B&ogSbZBcI6ct5luSksuf}Gkb6p4|D)QTA~laR>g z_k4flVCo`=Naft3*wv$eL00*O^WX|ts@Os)h z*(TLg_I(G88~6DvFlxEQE3-`MyJi{x#&|LM1}Ww|hUY&;{V~`~-ZXhkv*Ej9KWJC2 zsx9L4RUeaZRnqZwI(jF51`K_EOmhG(6YN7ZEgW zSWoCzm69(WAjO;=W?H0!t|r{1G+lZ?H1=>A zH7k@{*)!{9pa`?gd|%=71-52a-d^sLcD0)s5YWOEv*P12Q zlo5LgNM0v-x>J|(^6TR0i=#OAKR>5fPj=B_Z&-d_kHC*3<))Ex3XU{>GX{?QW+*eZ z*UZ*f%K8SUf7CJ;80Ir8c%Ao}$!L|qGipc8^eg$vZX<$gE&u!JJwElk`zF2l-NysZ z0m4zy3p3_pD|(*pYmlW?zbl$frU(MC6ybWN}6Z2QJnw6{ewZ z3!T~}bM4h|r|rKU5aS5suNMDW10J(LfZzUyWNGY|vbiI!GfPP-R3tadS%2Hqi7%ym zrvA^lAb}$}++)X*r7*BLe;&H?S6wTN+S=IzemT+VkToZ?;wK!IS1gewx-xVH*I}Hf z-{0f|7nk0OYvFF@I1eWIEq2hqOHf(Ft58GAtQHIysYgHq5^4Y= z{PR~lB#M@ExxjCw=mmejQn&pys_$+2nuP_@f|y451+5P@2`3P_AkvB zC3nLl|s(BngG4cEkQ;82Uq=t`^GG4|=N1e@lib>fB1mNX#8y(V`Bwei}7OZM{>M!tso`A`cO}azHvro zrLN>+A|ZcYwAcDDb(&GJ-alVVd#QAKzXG6}TetfO*1Lf#Ru;)P@*Bh&aZ``>?e`}r z8_xCat{VkgSz#A+NkTd(+CgWTq{f>}ZEOVwWnV&;g$G6JDU)*32l@ZaDxV3KNPs+;1 z)`$0tZ4?4sli>RKn2m(s*cwh~Gatz|`y2{KJmUvz? zJv!Vb|KG2lguD|TJ>T!2?f+2Tjfh<1eK8_F2ro1O?}YEa+uUQb%IMs3mzKm(nPHLq z3ZU|_L#*$R3$`*%CsjP0kG`VXtlj@U91HK?$dMV={l0&F+YS|NKiPc}{|>9_{&*Ak z0&BfU2FS<_NJ=$G{l91Y2w(i(t{{VBCRPCtIandiw%_juFE|v^(+xC)qA!mQVd0+< zJ%_s}mZET9pKrzm$wIQ7G`wVbi%~$a7v}x{i#>kGGLibOwuoj&5B*kK3Kn0f z53*%@u+WNoj&OiBS^QX6k$1uI=g zcjj=vS4rz~zPS?(ZsDHNQVy5JS7{hfO*_Ks8fke#cH$$oXrJMt5z|=Nw(Y(C5YqPU z%Qy-qE>t$5NM~o)z*wmS%pIg|i1tBV^?;4LbrSR)VH{$oI)h%81QsWjz6NzaJfRy@ zA{=9*k(@N|bvNp)EZDJCl@1M&y+u|pj`3q4kZfn|?Ad({+Bt5e#`E$I>E1hQGJ85- z*ePhH=nn;I1c77t-B-!5O@`mKd_N^K4zFK({lObo_21vO(P0# z3;H4C72})eIryWJ-H_6wro)Mg8PdhC&9X&r4EH=gwDl49A^CU(VD*6wd@eV`W%gNJ zpd6fNkzy`eB&W`R)_A{ZY;L4xO#tmqwO`om^I;&hNh+uUZWP;i*GNm5t?=nNX3E`z zB@>T)k<|K~BxMlveeFz}mBnG%;6x(f!$RO@)wKx!*iGuP%r5iCc!-*LZu0&I#49ew zE@P_Xxs)Uds`j0n2s~ULY%w{|X3)wCtb>pZ@!Mb?UhXkl5&ywjw>N#^8aW}k_Hzty z23fgY4%B39CZvGlHzYk)R_qr1)-AZl2&@|K^{y<=tpmLJcVlkY&FTX~IbUeCr)_Fr za_?J=gZsXgU;hjXjGQM$l=Y-uwk+vH7Q48aOD_n8CY~)4Jm^txH}>oU$&YNoL6qsz z*AYHwKL6Up@#gu{$=3J&%=&ROef9;BUbC-avaKTW0Bu9{j|ti$c6Ij59)Gn=ir-Wl zwe$V{tB?GG>3F;P^C;)*0p` zp;-+n@=4C+?{7ICRcD29vk+khtx37pvt<>x za3N-dg3H)DTgl7QA>NNfs_)P}&>x@MqgOj^@=uztSu5PlybPths++73=uV_d8`g~66?{P-0 zT03&NHxV#AE{&OL)X`BfQ$zNTfP6#&L6bWaD+#BxXW{hAp?Nqqb&DDQV=dPQ9Ub`{ zxd{}6C*|X>8pB690bH2<9HZy7jg&7C#qqIz`xqLYuE4KXq|f4J*8{pEd{k5a$h(DG zc*4zUUO#t?cc1<=tK;{j))=gOph%6;^hily>4G{ed!XbZsX2H`Jx$TJ#gpxbF=Pzp zveCo48W?7fQMMTNc}hdW!JBmCgJuZx3bUnVjOpLoM@HW}6BT~|6JK{|@}B-fAJyj6 zdT$wrJ_yO3BlIt%ZgBgsyE^i=R@M>(=j%OIzG{t_pflC#S2QPP!hTu1n>t@zaJloM zp1CD<4%vzxKCo?y?%`ZoV+Ly1A)%IO}j;|Fq8eY1I z4d(gG|1JpF;##(eMqn4K?GSug_tR*HEe41CW;y9eo~~X1dY3&N)-~c4QOyhVQ2}T| zUrYd%Y2##p?T(dsUV1b+!^WOE3bhqCT439MYuK-Fg`*5#N4mO)mlAj8Cg)rxHBq^6#QVWBn(Sg(IP*531JAU82iv z(gmIX(CViGO{AN-`x^}%RHQ1B90l{FiR__j@d>pHo7HXif3Qz-bCSVmcx6wm=!1v~ z_FUQtp@HK7JD|#=hm$M&alK#;``Zzr)lVT2n01(JQHl^P6YBVWNur=$hO#G~DZ{*1 z&>lM3X(S`C((N9hnm4Rm(J3g(tV?}y5O%J0&^7FNcaXUp6)=lnpG`f(JA0-_-1SxM z%tx%_TX^jbeKE4`k^hTP*CKqnDuI27J2@8naWnFxHa#KL1)e zqa#fGwd|@a08)fD7aXS@rP?IiMHq!QDiv=yJQs_Q4%zGWpF)YN=ykfpsNa>%;~N-}fpXq~UEgi|t^g+&I{cdX6gj@qFaos5*lKINNeH9Y?Q&~(Cb}oPND3pJSPa1|m~z>VV#EQ*nZRiu@bZWG zDJ>bSVW=^=V4@Yw;G}M8e`>E%)_{Kpx!O>*{!4y&)Tnzw@c1@3+azpi+|pyY!4#-e z8_+NJu|UiI_T|uB5{3((NYM_7VbtrK@ z)jVZ;aSiIp`lW3>E_FWpm86YYv`-1bwh5oNlYobBU-%Z5)NYy_>>MhTH4?MPgfKFX zh7!At;$xQKGM`CK;=4;8uvFYO{!NYiPH)m4$onp;1Pf{t;k@n7-u6xg%wfhYOD?u$ z+Tg@(G@)FRLZIYA>lY4wnz#=I%MZ+&#s^nr+7g1pl%Zz(BGTE4U6Hy8afGEn`0KTXbtTHU058aMoYIdby6 zV)w}DUfkWb`=L(=!M+5j1`jeTaZvA5DYS&x7dd{kuuZ`I@2BHo$-z2DY;;_1;0}z0 zXxH@RXK#QF_S%VlX?9g*J-*o)pT(Dju~j}M0ij=&j4`gO>=ze9%AX1!4O~%AIy~H!H4o`BPw7hoF=@206KS`w7W8U0vZ9%#7{Bkosy&W*)fmy3_MxCw=UZIkmWoVu(GIlYNy%e(|R~vtmv)-d|S(9 zI#rz%0pL*DdLS7Nf57^5RDJ_BJ!E9i{HR!}Bz%!^9+V@gsTikIFc;_hZr=3$le9by zFLyfXMMDe_Yb?fZtRc!x7C5Njtf49~kRwC&sL3?*ZK5D_YUmRHe+|TI+}FI>X6sk zxv{tgr90|9c5Rz!tT9+xRWut9koADQXPlvRuA!IE!fbV|QS|Q5clzH?5!BS1_0Qw! zkSLjrT!I@1w{2V?*ca2|rFVLBn;hI?m{^fr*)V&%5^_S*+0fAqP}{eD#ab3>E6Z

zIhmlJ28sd>BwJwxU>L}TGB*8TF}*F-Pxid_@yD?=Ao9!}GH?2h5~y(+4gkb(50$3P z`Lvu|%-IBp<_GL|4x}KSsts>-(^BL%f}hxRIy13t4H+2`-+xvSFw13j#-{0r`TD z=iYK0vlWTlw$E$_=h&9fc4hB25*Pn(+@c|(54$+FNlPVMwRL>P8~vr;fBZU%TWss$ zMAX$z0$7L^arz#Nc!V9mi1;niOYkh^8RIDLnl9JLpX|AX|G(s}KUc+2>m@J5BctfS zeBx{w=p!R(vpm?(G(ZWznO9S?J*K83md%$gbK7YTopE|()Zhd7E{K@pP^)Xrgb@*d zb8fm6|Im|llWx_x_2@7U;q{c{oyarGN@wnANpei%7)Mu{~=g9%tcw$X=d1>I4ph&-+g+x;924k zi)AWMWtqbKU(v(P&0oeEHBtV=`I{Hy@{2&wjvf6)x-5No)IEO~?OZ!V!%b67x8)qY zk^9ORfxebq@3x$)_wvU7e&Tiq)|otkmx-lShofocCTHhLHgXOs_%{sP{&3&(K?Q89 zxTb!bWdxxj^c?D<9}(YD$_UMkc*k}5m-i>%PaHiPYzgM{3=dc9Wx==a`vU+7v*!$8iw|v$4LK(OnRcySd^s-_(p#hs&)HVx8<;vb90X zGM|+@QI=HDxii_EbY7+oL#vCBeUuw4W-c^{HqHRP!<)KQ-4K^iN7&o;r0H2V$j0f# zzP`&*^M!oV!SBxCvdbEwH3Kc@rqy2et3g3kq1NUBfS?jI- zYfbl>+ba5fID-J+XH3r7`qO?n2H#WFx=HisRrN1S>s4S^>4C;Q3_yaxaO#~4>?g2) znv(olYRse@4xLM=>$X-UAc6ps+P3a?E}q4`)}PP!8|6M5H`c!~wW11EdE@%aai@cJ zmdW9pfT7Wz*Ma-9kn4`y=dU&f$#v`flA|`efXOD#p@*ll5TYN4BK!lEA_U<;8 zrzmJ2Bh_@1#H+*9W>|2##_-EZf*HfdcVs)HaQNonw;J$h15!M!bDm7WrLSAH<6Fl! z(X;b(ZqGm@C@T9c$`_WH_|53O_PLe>Y)BcAft>di`?1Y-TK9*>JP+? z`6C|QhNq`z_I9RkQ=o@3D$-qPjN-asNausO;ga;dmy+R5LIm7wxsn*hq>w*hHzH9csU=U+=zR*35 z&6PTH3N-9za^uH6Zstf0@~c%|y2{#2{=wEy9R1AZ-yQ$2 zX7mYpHjn*MqBG{ZR`X#^(qYcf*%u-e1&SbkV~D%=!lIr;`N;2-3`T@W9V)PM6E<75 z%B89oyuWlwQ4uz)RqO%)|yQj5-NAc7z?h8x3^e@5ukeL;odQ@oP z_%r(1vterB5RhncUYfF#lNM`l^h)wo1t%n1CKH|u9dCzvnG;@v*J1VAxaMPfPv@oI zL}Mn}ecBmY%kzuiq-Vtz5S7V4C&!VyXL0i=nO1#HVmL@U6=o7@NF8X9wm{s{D{qT< zUdutu58nUY^t`mnS#u+xc7K^Dg?lzG_!)KiMQRZLGs%4yKR}r;XmI3GwcDhue+|nE zX`VbvbG}qz(U-N?+_vRoCG6h1`|Hr`vjcqOq_aE&^eo)t;+WvpbNG3EG2LiNK|`4{LLXO3lTYOiIBNC8CNP*K8spmO5rA@UslAngTnI0r=Ct;;|Cs z<)VknRmvNaTszV}socO^-|W7s$Vm|1-VXNobxWtY#(LrCEM;0Ob-=BLf=xhqw;lKV zBfN39D=a`XS85OiCkvoEtp^3%Fme7543Z4veAjSI2T>}h^JOmAi`6>(Ldda zIEb1w$VKbCUVNRlI`~2%CxY!P|1YZtmN(8eis{F=*8Hjii%_7ay}AB(OVh2MX;1fYZL#7Bn`jAs4)JDa`K?eYO0m;(j}6_6;4daL%`xGHYn=N!Wk3YXvy4md zO0s)z-I7M{RliHiOXa~l5)*d<9sBSAf2({U7wTmC{;EXQM!aGcOW;~)8`xsZ$aYFH zqS|-$pN$jG_LGk)i>avunl=??ZoDCCbJAz~zKvqnX>Y`_&Q94a4(SCZD;D;xTM*rr ziuzehGMYYkzQ38+=Dd)xR9zJcB}hdB0M{L@iS{g!d-w;btrQmOw#EgOQQV!~yZm!V*T~6x2g%3|TCsHjvf#I0vENzTYZ%=)>Q8 zM4;h8pRhq^{m$GNdA$P|n{l!EcWp}7&~IyFf($;;igRjre3r0HECGB=%CbH^igIG5%U~5xotni{HE~4!Y}0!AXpN?! zM=UE^sDQTO-*G#1GcY%rH|gQNhDQ(fJmrU`s8mPjOh~+$J5OzGQYP(8)N5%udyAqu z=6$=h_Oa7bK^|i3kKH^X;veOzQZDEYRR)3eZdr3~I{sdu=2+|r&ZEKXZL#=i z)jzsg(M3mV+>wNxdvlDJI)`CXL{z|UH=fxiW4JBOSq1M5mGEh1qa~sC1ST!v@ILaF zRltN-wuBarR`SX`FX@W`LUP2LpC^^wcN=!rGsN0egA?!-+nNz+m9GgU1|RS>Yy8u~ z%c*k{Jz@2&%EVa$REiVKgG=HCmYW4(+n_-w&EXq{*v;p?zdB7g3ENG313H{PIpx1Y zAV4se_o_qt`>m*YKwopi@$o8N8Bh?v=mixaw@l~SQ~gx4z%a<{LZJ1yR%UTMSkoyn z^5E8yMRf@5ke~V~_D`RXY6O4!_&KreqrA&5zfbZRfrJs+)s{EZnQfXMSF7t?ytoc% zxVL!iw5*PV8EHrG9TyViFxg!(a;aObefPM&3-?*K?9moZahf$*Y+HqD!c`n)2i7## zH+gc<;-yw4GbXg;lAZ~n#`VVWW#VJnq4911g|x4KrXH~poltf22d_V)dN8Iid6=-$ zlh0uYtW*OCx-)4ps8F&+;kD6CkFDx-KfP)@>IHFjma%8c`Vo~Yt>l$qmGz_87YIg# zhvyf_e&ng!qFVbp+WU|4_mRJbXM78!g{Kct*Z_X^ueqwfE|yPU&pr>C?sl;Cr+yMM zhvdlE!>-tA_Q|~q<9<-H3>>YFK&c=F$J;AScs5@C>3K4w^Y(1i$#Qk9;kjOatYKb> zU?D7P!~@AmShdQN=`9)EfgR;#*)c&5CWtDJg$1}`hm6$Gr|8>o)mhJrp6{6%kF#tt zk27bQux7#up4zqFBoQAr`iSa73)?ksce@Kjgq=v~*_>Ox{DyK?piWi$2VQ%7j&|&6PnbOUs#1|MDz;Rsko7P~yKDshaJ4>SIlZ(NBFe zt<;Z*G7@IfXJTkK4i`heT~?B~%oJNy`61yo0n7;FVucMs-5B#`rczlSX)JS(y9eEC z+;W=`ECh}M3to}dY=zUTn0||=ibB2L?BUBas{p-d|p{e5m4`XRA<;%nj7mKQyfE&iDKicn?}u2|1=GXMr&t2w#DM#un+HoEx#P z;aNJrD8CfQB4+%>3iLr%2C33;H5#7u2(Dnfe7r3)S`jY!cbBexc1I|Qa_o+iS?Jhu zl4(ibAQy17R}oB$txHH+W#)dJZmw+W($T*Y4~;3IcV;rY1{hYiIF&oDc{9#2)iLSD zUA7z04tE8U=nb_NjAe5)#$VE@fM64?kFa%XTO8|Qzij?@vN#-NX~Tj+{Q6x=Rfx!0 z8i9$e_&?Gr+cP}PNrwWK zSvtbet6|c8ZrwFR1h$d7^DTK67K$ZCOYxNjXUVGmKQiXJu&QhBNW^pWqeE%H*SZ9@ zYE;ku&rD!m?iXr!+P(r}UiaLz>Go}U?+C=?qWp&i5c0tt3*4RC!y?fm!v=xFQBt|Z zHUz0{;n;+xn|W}xrqt26XN=%J&uB}f1KMm z0Ff{h;>+vV{W5-0muf=wym&%{vEKPICG#NvfeO2ttdwPDTTj0Hx-7}Ekk&2TpLkS~e#!o=WosM7rDzs4eJwZrtY zUw@ylk+5=DqO7+y@HwJIcOK;0v>r&!$90dqdGa;oc5iNsEIJ((0P6}lmJZ9xH&ta( zs#a5}>%#McBbWVHbHAajB&pJ`T&EB-jHs8M`c}{#U5i!D02uQ}rG0>(sl(HV@#;~d zkFnq>pv}*8(_S_gJq$`o0t~Fm=5Q7!lF%e6ypwr3h*3?hC-C=;D1g9i^=SNVZT5_B% zdTy57x1P4^Z2Q9#6f&sqTQoZWHsNKH4VUst+Uq42!5AZCW zEZDMElbBckc(~ZECF~|2Bcex@s}2e*e&9xU3PFDDvM?I*{o}+WmYF&xa&*7N;c1C3 zjH~RR>=?TNF_d(qHiKlAz5m130#M8=B(HSabz!?PWiUmeA1R282lO`W%E=#49YLR~ zyuyZ(v|NEhYVLv+`QAR576=%C_Qv)W*|hN>YDw9m-2t3wamS>?BkB(F^18>L;{wMj zSIbJzddQ5nc}pn2pwrV@aOEgH+I=fj8p5~iiZq}XZ`8fTKh=Trhk`uyqTMN~``Gs_ zq~VXk=u^VS1(%$kaI1>egSRhu3^>3h*u;c3FT!-c{t|imoWeUIAR^lTD^aZj@CTu8s@LK*)2` zt@r5|gq*8sw&bQ@b*Ow4D+TLE`4Cu7`JH&%*}FC+RAPqTwWA|O7sjI{Eh|;qJdhX+ zwe~yPh|GKoeOrR?j_Vbi!uIaqi~}xv1Fvz7E@H#(Hu>M_nP)LeRa^wD5@ew+T@P=4?tnn_B4#{WWymFUx)eSYSl& z+;CBhq$To6%w4N7vo8ba_V#0DPf!gRz?d-!ObQrnT2u_FgwYn>Z+{ zZ;cZ1nA&``cKri57@Ex8Dh+*np?htIiP(R#SpLHzc^b_vuh%`WEbF{Xv zLpEx_WZB&4?qOBOLcf&z`C)x^8_%BBDwB%c_j1a{m6B7?AcIfjw5>$bT}>cl_&ZO0 zb>a{b$afSzd9+>nTH|fBaS;4P`on1?z3(O^yJ$wr-9pK5tpb~}9K0S_&njS-)3qk> zaH=vL_l@q#!7Q7+)5nWYzW5VCQIW1X;aL!^#w`SfW8gGHfX{};tnC}gcLp$Jl6Rfo zQTv=8UKNJuzjki*6*^&xsD1UDVab8b!;)=o(pK}SrzNb%?I2oFHveivRW!P(Q4Bx3 z4OzYD&V)9vt8bSF`K?ZE-A!ov_kwnvN&kJHi_hk(Q#H2S7ys>7D=t=1no()p2{Wm4 z{y1YMl-&;XjAJYrfz~8r5o$ycauF^Ah%{0#vickU+w*ql1d@H|>7{C%^XXu8N#okwe{?#cH;E~o1(!yf0M zzkX;AHHa%NPohPUMcvMUE4fm+m>?5fXA?a6SLK8K2f1!K}6xiss3|Ax-pwD z@}>e6li{IM<7OGN?QY1;yhK|o#g|A~tRz!%)*(qcUln54)Rn6-o_G%AF)S#DS+aIj6UupG1>Jufb<)ALocCf+@u6By*59k548~gQDZ3~cm6GCo$ z?vO*!uCvq@$7X|{3^pHLU5pu+5=!>*s-Sht)$pP0TX*+x8h5WhI@e#hm&@$t<0A>^+ z63)LqGfUmCmO5in{?ox@c-5``mbpZf2(+lAb3UzZ@@M;mZxlqn(JkR~N7*P@H9rWJ zT2naHiIF{zywK4u-4b?7tN+()8Xh>^)S-&rXKA~-2{mz?+K?b(6k&(*3SgsvTb|ZE z86gQtk3(nncc$VM6-M_zcn$s*kC_Z|B+a_P_bNGqv$^Gubo}kPRj;$3;q|UGpMz~B zR{(5~H!OHNQuE2O_~E2ElwhxEIIHrG7gFxFmMTv$1I^Ht%G<%Gk# z24qoF(gY#bKi9Ec%28~$!RdiJd=Xu|W5{AYX#AY>TIJa!N*On-7)AeZ_jyfBfpN=8 z#o+~S3if=ut|zCeVH>oZ6LI6tbB|B^H4%%+klWb&?m_DC;FrB{3+M-pnRll6QzP>K zV6WIq8tHjs#9Nb#{*K2d#HN-GMg)Jfn%M*mUMKl>ty^{9sV^GC2y$=yGrSSxlxL~p zSp3Fh%190T&WeA()&|}7k@|<(!K#Oye^wOS((sM=%}2lGp=1+X_VGOHrP3JM#niL=?*qFddIn2WLkgHl*`r#@7%FuCs9 zGa)3m^pSqRSY*2vmv>25JY=-EOvP-letaGX9l3pHG{xs_eBj2#TAODhNs|Xd#qJJL%`>Hn1{yUgFZU z)-b|Q zii8d&oFvjlE9)Zc`Vf_$gnVas8zp0K=;qsN4TCrN*D}y7;UMzb(SNwK64zeD#?!yr z2{8E0w)IyyV?~nX!{G5ahH6%c_xM6u(_h3mBfhFuOgEC72HgusSh=_XVH#@=%$XI3 zBAK|`!5;yY+b0yagHj$Go?UDU47`fOpyC#7lAB;_oF@|%B{zMa>syGv3kg_!fEPAl z0pIrj<2T2S1hVTIfmW$T1)SE4M>oqzmbxVLGxCl$3VcomYua}lM$?1WbvzG$;Mxj# ziWWZ1j*06?;!4&vHwa7;F#rxZPi6L6l|3X zKoR(naW!dY{wTBBG#W(&&?v{IkP|fSx-4c==>4o*{e6k-H-?DH%~YF)c%$i*8jTKj z(FNZqSg50H7!Nw;DLvB$PPJ1RUCYuEZOpj%bvwQ8&S1gIc8MHro}mJYts!p)213h z&Ds)34YbhIFI3(eiO(WZXBAu@ADj9FuRbxpnC2W#$5Y!G=RTTF_vSl?l6NZSN@B#) zdLdKo8|bO}`kVNV_Dh%#<3q3l)mTUAZcNu`os1ZoybotnaL0CE+?E(*uzo1n1)^m>5G?^b$l%nEe&Wo>J2_5wFSCcuW-)VUk z+-4MMvHGdzR$B^F+UVi){;^&@19S%y9Ou?qcCny7ddB$*L;S9dKtjA0J4=&pA!|ij zZ@YI0cZ9lbz%1}_;)7*254&^L1s|NYLUqquD)zK7Vrl{!dTd9$C`Cc(~X44(MjYTU5Ot+iMVsE6l3U{dt)1@W=`~W3lc&1(XvJ4JQ$@L=Z0yS?S))N7} z_^c+Y40FZs+#kd+Lr?n^OVo*@EaM=V5ew?u?#c0x=>E@w$J^?!Mq>&b(NWRwbSB?L zE+A$F2`mZDKkX@vhQqkSyvQd4`gNNKM>_y&}Ed3+z1(-;?aqZ4HJ7!i1ZPSUmuuft^{OpvM zk7C9bu-nfj(q03vxld0L(!YI%j=X+h>W*Z-V_EAfm7i@)a)oVH zYUk$nGdzb|zNcE^km$>g*}Z%twE}iIaZsvZKQIkGP!TG<#oqbJYNCJ}h2WA>>j{6gI1MB8Hm^ ziefS6nTiY`JkH?Ls9nIy@SwwFa`$56#O0XRS+-dkIRF0Zb)SGd^OToD!sg3mg9Q;;{6G5{aCB+JWC%u9`s@z86kVN zxr4~Jv}tO``@mB!qo(g*&!_B+47wjU14~|Fw$$X9emY(Z!gHiHt>5mR1xrX-dePL> zujNb{g;be5|LQoGvJuoBZU00Bu>NQ3ZPt5RIh=8%so$%+Ki zFMW;UZunuTBlHlt`bRSD$pI_@$uhcAY}tIiQV^L*Q$AQ6IGQF*punZGb)ZG~%GkQw#iI3}{huD= z!et{{t2=Mz-7c{JYk_Zj_<2A({i8Lt_y3U(lX_xRAQIYYse+&$2B!*g6UfI z21E8uK?Q8-8A|sAplUa-il)Q5B6>73ugeknIv?`@~=a32QvY0)Hpc!|M_UL}#1_<{p=N(&P&|)J zPc|$wt+rjV1hO!1IU0ROw0j;R$ccPio*V*Iui(ueV)#P$kqvUXMj(+t*R7c0BL}=b z7i9_F`6V^J9#2mAQWSrL2hfC&tp~dOMSY1toI5BI|EZwshHF{{3g#tFkpc-vK2tu} z0B^?Ipe;kj!7n-h-DRZHsr}v7iEny{60sRchg!?av(ku5H=%pRPs}<0;EZUOaII6b zkavx>#wQJ>cz*C@nb<1v|NV4`0umC9-nZHo@pLWl_*M$Sj$+*xJvL@EPr;YF-%a~U z3fv7xjC7CU-hTNlES~tnNqD?w$WXcDd1QUd+7tV876n@i6(a9?*(XdeSI}*o4LF?> zN^{O$D0{7*cS0a9x@pUO78m!xXG^2gY3;}A&2~0hobFU@c%EluijMHvqGJ}!rT%o4 zZnxo7k;^>QE3RL5#P@%)zVzW}Dpp}1pP-GB_GPKX&ETbaIu-u+Q?{(dJb>F-@Qea9 zzR(D@Gp@uo_v7zAPxBBIwGGU~1VztO8maw$5WUztuNYUwuV{A)d9qUZFV5_fRJ7FA z11;p1K>s5uCN2of-B_MWd_U{13CYu0Q0@#mNQ-q`Lv*5MtMt<{&)Gh$$hq zx%Lq?7oDG7_z+?Hp(F^0|7g~HRtjz&gg|HJbvL}f?46v!tO>Hn05myA)YYr}1S>B) z(F7c+V$d;YIean0a^h}<*NxgfdxR0S24GibSkIpS+S(XuqgtsSO<$Lrf13`N7-l^_ z)vYjj#p&}$Xud?#c)`zoV%n%<9`_q}w31Bj&84@wQ3xXGY0vcD1M9c$vv7 zZ?RdP?!t0c>M>H3dh$Go5h2qbOZSoP0jf}^)}w}I7X4vC#9MY2mb$uAkCk$2&05z8 z6*=1AD4w~C3vf0Z0ARy%Wk9Na$(^_Pu0p%3XC%WW(0X z!zmQ~zYDN)kq5g!4D-?OntJgs3VcW&MmBpG_%vvnMrU>K!S$Dm8VE)JF`@l!GF^MQ z$k3$NE_Z3u81^FL(aIY*zkx5#HxQoGhb&do_|gyfP`ZD*QGG*hvER9^iN+EH2md=m zgWC>X-;sN`5sqiY%@Oz9PjuZVFcL(K;EdRx&g4kEiz_P=2HH} z-OuSZ?9@3hFL}4NK91%o`_b4Q6GA?mX~#Acr>zPvQGTOFyY?(ynP~BdK#q_|FJ#V; z&rZDgm$)&B+|iDSg|f=zf8>nZ4s`I#)j50zGy!xhjHiY%4groCiv;c;dGjWG8fR;%UW|6$9{f!8l*)o;uUTX3+Bn57%o`6=t;s!n9lOBTq^jZ!RlbNTR4qB%* z;&cO$A1M}k9t--b>dLwFu=0FlHo6KRU!1p|s3nc9WHDEiZ=nbtL4_o-<)qEx-Cu!f znXu{(m>wSLOU?^!K_$8*>QdWwY8S-lCb`k1j{{E46sUw}J9G&BgKMOwxwC$BHU6MV zeSVj*RPEo416kgWz@FL_fG)Tp9x?0XTMdb7S}b5%@a)Y}*wgMMGE3@oM06=~zQ$y| z<0edktx$`=Z+UI@0po$-STFqT187Q>cE0t`>jKcH!b3?DYkjdj14ZjmefBZgG_g(3X5Q?$mS%9hR`!FI5$9jORYy(vYsFk3Xk1PxRa?T~&yosLJ$AuU4L_b3WB_A$n!4PXS zf7#^r8EK2xa;jTF40?;kY{-{j<@qxwiPL=SP9#cUm9g=V%ho+4vdFKVo_D;N0tqD&x zRfSCchWPIn$LlE25RWpX$~D3e)|o>kM`?jcP9KP5o$i>|Ma}Ec^$D9Uy`y+xUrbnG zNw}ZdtEJhG>VXY;H$M6{kM67IrHC)!FE&xCuZ6#8XpCFB7Ght399%w7aYIn&;K7vb zEIFJhEE$SZh!nr^b+Vw5e7Ey(wxq8&E$RVWId}C%}Wi1)aZA5IXq~RXip((?#;z@J#a4&P*YEjO+Wc( zs=3VAW0ZJNsS*D$H1gve!rEEs2R9nuYZ+T@MiIn6f`bDaNiIuPH%^SsPIdlswDXAm z#MR%?+hdAi-j$8%V$GSIwg`@ul2!3}G7WHVe(`0eD*inepEo!^cPZ6QM2b`h9JURi zkm7N`t>bIHdNb}fv5uxq61p#)edAcZ6&h8rFI40mZBURO8mE*`W=hy6MW)xmIU$rQ zk0++B(u&|3PhlvqPB~oF@qO!BT;|=|S{%DjGPM#(2(5aXi-=JIHcR>1)4V?0Oxv05 zsHtd+RU387+@}OZM)UeEd;idUvw(|x@1k`I_?GZ`>8Vv(+fe>9a>OO+ljjyIWW4>U zyt?e$u4&opyR)Lh2`Wx*e}8f7*z6c?7{J{wcqimcN8q*}h-CmZTNv=+Ugg zYI?~{#ew72i4lvL=5PnneW1{-0h!l!S5tAB*FG^`b>pks&RcX^l}x-zHaIs6dwwda z1+#eJ=Pm#uk;1a{!W)@Es!qPD(Lc`-as!@hW`ebph6nP?j=A?#HD z;?>B?Iqf28dgCUf4LdNwGuixn%+Z2*A!FX}8BJwQ@wB$hHK`a&N8_!hVC$y9|!~u%>d8b+DYhb!KI(WtEJX6jb(!ibdWO zLjCnZ@3?_=oS$yCv+z%82D)I-*WZ>#Ec}Byf|3mXESPiG4b%Fe!tRk5D%C|I5b-U? zqipiq{MtFS_mho9NQOkTvGosV_+!CZh3?GC|k#FMDcJM9;aYF>@kUr2(sUjGA zlEzjSFAK=BWIpA-6}^+L)0~C=R2;}dULl;)94hjW9u;aRPIw~voAuS^9MYmv=cPX2 ze}kVl&qGSG*9>HSMs&}#x#~!j{eRlZ4--gZO~M*&V?4&y(> zSZBimpz70o)tI1Bb60D*V9Dm6!D2D0Ed=O^-_*7}8nuQot-lL#TbI-TmP@y7;6{yi zH{7(w#H#WCa$?3*{R!!n=88H_Y&L8yqB4vguI&wQ@2XVPdmeglzM6V;h48$jZG3TU zY%&IpPFx_Y*k!b$ZfwLJv22i9#mGx;eSIi!fkUPL9Q%4q=KM2KoB0i|+V9og%y{xF zw%bh}h=00gqdwkpr*=41-`guP5=C| zPt}Rn^Rw!Hz>XeGG9yUoR^ zs?w}ax>DOLd;B4V{eHJREmpZgc~;$o2?5ZESD*jRtNv-n8sPs4THy?_v%1DRGh?$($23l?=ou zwGPfHrH^@hX#37xffZ}t@$=GKt7^ZqTAkV@{#V#L`7C3v|NH)!r9bCO=@CyvDsGO+Wnu8mzzL!4Bckxl2Y-t4@6AQlR$Jw+J6V^5*f7>e82w-u;WB_OeW#r~Pjjv=p0_8X%n$ zGW4)kQHC(dxRY~uza_59Ie6x|w(JP4Oj>-AQxSr=b^alw{#WSG&9H7I;U}R9ak-?O$7K z;bE1O62m}`^076;dQqSEBJ;?zqN+9U>kW>w(UuQ|!>^I)0gH(>bli`TjmOQ!jx0|j zLv%2}ClK{AoZDq7x`Q}fl|X{Ev4id0r8(p5VcUG!8D>}X^mVye*6fgVj)IB%2RI%j z1do+j(#md})bsJ3ZMof_lIX9$(oiR)He6YZUqiK(Na!z*;GAley;~~Sn@mqxXTp zO)0QLIb|BOGu$c!KXkh4^@J~$+TRgS zG+wTZ>cbi0AkgbjEoDjLJ8j{7OSS(6KccqiLlt#%rGzCJ0{7o9VN(aV1AtAa&b|{6 z*7tZ&9Hup7+20!(IW`#@*pLKKK8?g+aD=AfsJc5t^qA@F6Kw1VTqGAIbbJK6pIu(e-= zFBsT`HlJU3(rr85?M3D-73A;OTD*}Dt|V&2Y*gaurvfZZm6AvRO1=| zH(R%ZdnjVNQTCb%1GX<<3bOKwIkH|YPlPR>IcK~vUcRNnOaEhCRVL}OjXig_X)QhB9TGqrf>ml#O@V6q$E81TTcCU67~Q4 z#c&rqT06Iirks$gF0By7#;nAghkUCCiAvt-|NG%xbJ2$L<2 z{1)m8Aw0zNw@*??dbP*bqu>s`t-hv)25rfo7d+&H@V1G`aG~1d<~c@zoM!!D+S-eU zV^^`sQE^^2{SNvcmFnTY zSx${%%po`crsX(6-=dgseapaQEQ*^|7_M})Q3@j7vhsZj9pCz$`;;8b1;SMuD%o`k z%~MoL#d2UljshgQaZNy#g+n#{-NOQ*Z3=O`FTSnNqY(3tG zb(HE65$#+fh*Z<@FI5@l>L|=X%!5;7aI-1S9Od^=?E3U^2HD?XHy)w3<$!AY1jfBG zPih^#AI@!>?1&O?btSfmuo@##;FUZ4bm@U3awrJ-dZ({N<9O>~PEfO7U#qHw!NYb? zK?Q{fNI*IDF9oS}gl`37Yo;P-+E9N|1@6x^{pH~b%PG2A4e1Jt5F^pJ?DUk}?bTA!Jx3K2>)5*DE6wd4`zqeDuJfgltqmw;XEE_|M zFeq{M5abmpt3p@Vg&iQ-wtW{xkoyM89srm*S-;36Z_+ZO;El2qA?H6H(16R?S^_k- z-#`tVx+2in%^mV{BxN#|?=g0NVnf!RSw}m*QI813^jY~U<|*yW%hflk<^|IopkiFuXB~s$Wq&C{ zP@tL2kxb0TVhAb+J;c#VdhVcXkFa;~=7+ z9P{j$B+3t|lQ( zt_b_b^UvmyBrr>Xl3_Fg$Im}p{&Q$*2Y-I!L_&d6lf}EO3mzY~F!MOefeNa5R7IV9 zS27rg{>pb*tB%fP&tpa?s@I#N{%OLh;}*R8jJOtAO9~cw1B!X=5zW?rEN8h%ZTvRH zNh1gQ)IodvgjdDE@h0yS%f_z^OM#FE8zq!>Mv6vdJIHY>R%o)9TGTXQ*Tp0;FxgG2 zSc9LW3TWt59Z7xXAS`wbH0NJ49QI^Y^m)Z}|j@~tY#+S&($ zO$7B^YlF;%v1PB-Bl!04&9xtr!KI-wZXSAzCnN)3i z_Lk2jtjwCFv)5F8ILt|x-eZpLCrh6!3sHB#=udsenzUN%1z!X1V9(9OYQQ;|dzL=8 zmVPUUMu}`h|^||X3cMDabJ0t z=b+3vzKJEd7&Fc4*x}M**fl0u$7!sk;j=xLU7#$heT(JX1VQR+SiSuD(uy;dZBb_c zeNB)eYHKbekBwe*+SFR}mAF>1Y*evqSEZ2urC_$Wa3qTEQd${78R>QOJzgd-9I$+8 zU2;Y`5vW-wNwd}7z(A%9vGz$KYLgsMU0rp-V-V9kL-=v$+MD;V$?i~xau(&F6!-Tq zU3KBO-T22m$o?chuVSEVw+oV$RddFo$+E_NO^i@|dosN8q9W7%LHfYtEO1!kN^8kY zc&tbjquhyyv#L4dL_SpB$s`yka$l(3uBE9}7&Xt3gR#5Ni$Ee%UZ{)ZB-@2Pq+Zrq zo$tlF#uS8QKdm^s<)}8FH=!3t%4;5goZ+kf? z8b)aQZ#m*j|D=}<$^!h`MYoalVWF!(jR3^{gJ3(C0CO$BgAmJpo}(o25t7bbS#iV%@QCL8wY@b9vt|+Ro|m^eWsm78;TBOs#}o8v~hxS4|t@@9Oj9 zkjKJIR|1Yd9GQq(+`_-AX{jFr{aB(apvwUN(pQhAA3s>0O?`bNCln0{y|`(rpPqmM z#Z1$!o0DU>0N$1RgtwzgU>0KvA(hdeX7#cx3H=i8_3rC8lB9; zrmM_tc_x?+La8!jYSi%BL3cxjWdrs0&4!Or+3K1%Vz5pQWs>iGR}k^JWtt=WF}XEk zRpq8V;i&JO53!dO|H@%*p&Hp8aXMdWL$#>%ajcs}4)TX?8wl}+o8JR;Ubkl~_*7)(4Uo74A4zimTc1Y1Ue}ns=4cGrUJuqOoE^R> ze&S?8?z9&D-wkvDLz8&4AJvgeGm;WRxlkj2c6GIs2X3965v5h70F^Lj%Jc;Dm#L!RLruXku!{{1~p7aPkB{Cqw&Dh2 z0oHJa6=c*FbY|sCvhyW&CFi1^*2)Zz`WDe5@^oC{pyg~X;4JK9!q1RhcRKy_@h2p+ zGvC`g+_W{-5Yc*Yllc%=tXs(sM#Z(!(W^0D{ESe@T$ER7uD<_#_jG;Kq577?h*jUm z4VOA-k%nRX)e%{)s<>fj>-obm;QWUkg$Gq7G4T%?c7OMHMl-B{xIICnnrAIPRhIp& zJQ}@ora;+IzrLmlIGJ1GOtQQ!{5qYmYX%A#Kqi_~IXi6) zziU%%tXuEK`JO-ZM^7Jm)ZlRmwd)Z^}(5x`j5;EEjR zE|Zg`(o+LR7HzCt-}FeHc9VdL7~CdwlevfVZA&F`(a7!Yn2;x!A1!7@DzxLd0Bu zuy*KgFq^8cXZl5pQC9x;1QqQkqpn<$#;FQoKzqL=A(KrV@3zPqC}mkbtN z(3KzQ7zzqXhQE5x$ZzCBVp|)Vsp`8q*39Zz3$hF}w_F83ms51RLaF=bg0^M zr1WRr#f728YKZ;SHVK!Nu(-7;zYsdCfIS`6RIu*as zah9fy8mBSdoCK+PPz*P#v2q9~+Y&so0RCMalrVp?snA;)==~wM+RfpB%%@gebctSr z#CFjJrtl~PCe*$$3U;w4X)gcIaEJ^KiBLXGS)bjf`CxzFFpSWBL^XmC=eJ+SMVCx##JeX;u2vDB;uq z_%TkN)Cdk*693qBVy3yNLRCI({BMDULl>%>vUPclVMVxK3o-lYAz`8nvx?I#taU)Q zFtTxhcUV#hccI?nFzSM1Np*a*5(bU{KZ%B#vpmLYU>E|7v4B?`tE<(OS0-WJF_(t5 z`)o`Y4P)~#0=bu4fh})-T?2kUWtInr)56JzU;FIbdNWhKcEiNSLY2Ik zbZ5&d4G+S4S3 z^hB=M>Kuw{^F1FHq)(cOLT(_oQ)i|m8fdna;uY0~Gx;SVi*Hus%}(U!=FA;k+j0br zM$kTs2c~t;Fuj(d^>;unY!f~CCW-EtBj0yldPx7m$`vf6G< znXpb#UTAb@d~q<&Xr{$GPvl}lGEy@gRV*3zJ%R|gz|6}=BL5I0fzzwYscO$sq(3l@ zaG?%HXAi53-6WIti%SsL(v#s`&9>3~_L%cHc_=LcpOFC515h#DMoeEmS& z?e^&E+QnT*p>-`|ZOb~Ve+6)7Tl!(7uF%a>3>n_xLx19rJ&CfMiPX1S5sEj%Ps#Tj zeLqrK&7F}+D2?0N{s}DN6HN-#1XO(Kl9mJSr`%?zbk_aF3fsHQ7Eft`>B4F)dVXGk zBi>Qg63h|1f7|gdHn=@kOy?yZL=w79C(Ff}m& zmA9?Y3e8m~CiZ>nXOC2rmK-Ard!OTkYeYW8fuKZsS2ZKPsm2J^nN*2^Sl1p98pj1q z|&=F2o3D$0PASgUYWrZ)YT+b4%Yoy?9|Pn2-L`yP#c;k z1Vb3Hs%y=80Ao}uF2li?_JvgQz;Y1e&jMIU+Q)`Pp>-RXRke1Bdk3~^KLhi-;oRDr zE(<9To|%e{x2&QNzh?}$e15C#9Thmbn!t-deU>Qe3ntYdz6ptFFXh&TH!JNwqDL=c zOV-RAX7lFrs23nMbsl<@S~t_vh}&xo8#f0x`(G7eC&)kF0l^bulI~X>Phexd%l!78 z?U~(qXJ}wt*V6~m(R5hZavZy9@L8F+IkW7V)?Z^6YCn0Q>*2|v$2*;G)j;TLd}ybq zFogCdA;gz1gaLrPIxW-7LT@dtSqRB#qO623p)Pypq<6Xt%zVhIG#!D{?xLg|!* z3Pq@5xxbLak#wb&o~jb&!&boT!KEjdxKqBb2a}ak3Kyq1WvY&W?ADtO2GUgR#xxsQ z4{SgO8($>9=TowmIL=(mvSvN3jdO2~aiz`}UaGGt+Z(f)D7gJ06DF$$*go;CKokj$ z(j6;Kx7SYky@R;dC?G5vD;hIpN}TUelC$j=p;)YLwE5-~T}V;#AMZUrnS?yZw(e?75#Uy-$soPLBd<;)tWAJ^(@`` zTUn>ONDM;;zylF!8TowjyqmPDDjRwQ*e`hdo~kB?^||Ut!-US zU(YYzbTY{ipj(Khp+d@ge;{02M?aT@8cLz<=E<`0e(UfO9{1#AwqZNbz^)DC!J+xoSWM6lY4lC!b|4T}#146u2J zZ!M{2A>%6yFh5jq9#b0BR+`(34Q2gBjU&tmoVT{rdL0ixJLN0J4>n}XGwau?repWt zk8$zaOR@{^`8CzP0Pmc-`cD|$yoVRJkRhLH(k3tK`?}pq({T2$0HdZ>E3GYxq)1{6 zZlF!=!63wL6f>kjJ+mXvD(;<@+4L;=;spc+w&ltQFSR^sfyPgQTO)$a(Nf#xF+3%g-ab3EYTjOhnvs6%pcs5<$<6@$PUz=d6S&S;K} z$8B7}Oa(3m)V5y8s|2?Y{bs_eU)DPUCWG6&mYqX$9A@!V|72B8|Mv@)>_Dt>Y@7An z&upkdh7*Ie1f7l-!35IlUS`-b(Br)t=nc3nrN<(F@zvp!Ukrll5U>keak0%d>OGQ! zp0FLKAf_7To`DvNnt@&yJlID{Y7(VQ;Vw}_p_t;;rVR`ql5ST%KU`HL<8-UA`|}-Q zL#Scs6-SSCp1zOqIzJM?;LWwvCVx4A4zJEd+qsNY{W?`=^=a*BHTr5pMOh)ZYqR)P zIiXszpguR_=kvt!pI=Rz*hi<`TX|4>>Ve-V{ehIT@s-?uet`xqBDz~a zu=L+A(GL3kjF(ILo#!j9uP)R$1LIIPi&XQh!?mhMx;Y{@G_ zDeE5ca+nJR#eZ{C8q<_o#wO*)n)Fby*i#b5r6c5b6%s6qnw1MCtM$)dIBA8u6?r63 z3*kt^SIX%tBK%c5t6QvIxJ#w~-XRvU%BycSbze)Sk?v{XJ-)pe?tOSdG+b0bbInw>Yfk&L2k7bagZY^sr#oLw zl&-f35@vNA-t8-cF{5U5ux=y#eEVW%A_SeH(D>BA8-D`)u8MlotX~&-8j)zaapM~o z*&w8jMAK5=^#OebOFY`LeEUw_D=>G5i0xap=Y;EpEFCV1cnGPh9RrO>$fLn_=4mJ! zTX4aJV|Oc`?K^V+b}Aa{u%Dehc9M{H<#om$yX*Fl|K5ve5&w--fAdf9s$eAc&nbKB zITab$XSG?_G;6j+R`3kYKZ2lx|HuIYNHhD=*N4Krfp(-+cH}qL2T^Z3l=|DZ2ib)g zQgv>kB`nvz%J2}ozCUn>W1F;R`@D9z0rbNH-7rDJ1$|et2HBLA?_(cmjzpM5xMZJa zf@iJU>R0s(QN%XGAHaE{+dXQ~#JHJ!D*H%5R^{B@7%)f7+Q2Y(&)@DaBLSx4By;~w zk{L;+DKMCq+O&%#IcJy(i)Y^StO6dwxGpwY9m5-Rs6t*yYbfdZXE)8Q3G4L%J1Gwd zYZe<}P?ipQQa_hfrJH?KsHqp`DfvcZ+`7O&Gu@#kN(F=Xi75qlwN2c1=Ix7YlNQqN zx7J_Bi#eoBhk|OeqBlV2=qWqUm5PQ6WBRyGp0$R^Dz5?3k~Y3QPIw4KVv5jUJ12+e zCykR-tnHBReb3eMEb?USuLqT@tsfHyGQEye(Kcn;4dqSt4kMT>7$IVu`SG8IEvKyY zUff4XN&{;D$N0q#;`dl^c1Y0pT{wdb108bMD-tvUobe=N7k{@@(lv!1Obc`~Bt2?N->r2fQHfR*phqn=v8LR^aNgB8I!X>Rm}Z zopTO8pSX1|INp7ZVW-;2nU49~nGe=2qB3lW>G~-%p(nAzN}+p9=uD$8zJIal8LOdu zjM=Eo90qNL((;7K9)_I;w~zYTeYVUs$~Hprj<>Dp0FTsjc&R$tf!Br{ty7Yi6A8yF zJsxIPd=sv)Lq+Fg^ftT*o!P(raxsjtnVQwO>e0gr)bf(b6%e{PEV+xy;>cF6JIty; znWX1EM?Kx%Y$huqAUyc+nE7zrwfoccn6qRXmGSYReXfo17Efz*YoIZSLtwj$Rj1F* zit-O zi*O^YkI)x!8;R1c*dqbKB6q(tUGEH0@9}-hHL`5YraJ3jG$m8~h9fdO{CB6nub7bM zYvZ~jD7^N%=@ij}NzK@VkTt_LJ4Zy@$-OR{rxPZq)HF;mE>HUW$zq$b_3C>nqkmmB z(+>Ab*SJ!Ff9!k80r@6MHn>n2U%_%JOq@9}OrSJye&P;pR=C}NSL8T#4;Vuz)@MxQ zSei}sd9PWMZOpRh(8!~6uAy8hHD22xFt>yFzTt)?YTCbY40|DcO&GMAr7d%<(ag+q zqLEz}r4E&Hnv%E}@_hn`2W=E|@OQ`tfiT1sVy=O8foPBOf#QZ&Uecf2>w|N7bt+y; zQ6sfpTlsb|CEi(opXrJwRUSiVV78?SUdL!=yea;}St@ob8#3V@}oT^&U)o~8;=uHfO(vZpqMhZ359Vb5+>_8eHh z-sBcRI{#%uMx1cc)>3zLA+;Wo=KfVN)QA-7fdBiu!3~Hc^4&!{(Hw=t64)gmFgIefg_YdjzCdV2R@i^P%u?LX1C$+`Y7%dKwpJ!@u6Gd~2=Sdo9?(nY%?RgUSGI+m(F}8kI<@Ll zqv>GOS@Sks`l?^z0?Bfaw<WS>;mPJcyV&N9} z)pECfOX8mIP3ctlc?Vixlk?T31J}whww1;I7|>ycFzf5;7;;7-1L|$Q5HQZPY%K^J z0daJ+uN)H&UEVgLGJhOU7Yo)i9}l2}$MlGGU5T7v0Lh01OU7k2Eq&*Ragx0l%`M!{ z`?ao6^Bmb*Rk?U^J?H>U@$9?U#dqyVaOIcAlo*qm#J6CdQigUv!IuK?aAt`lWjN6H zA`*o-s+F|fO4Z~%3cS2=0H};IllRN8XOE{LQ;4w&UW&_5Ov>~sFue3Jf0*p#XmI^8 zwMoS3etbfbhP1#@xKVcX?`XPgUeua4sZT*pS8hW7Z0~V1#vgcpZ!v6f<w@0VzS!!09S1q=?u#ci=gn6Oo{;gzq;ft4J^U`CdY><8P5-^? z{u_->UPF1uA*kmg5L`f)>I!KXH2l7RPTe0?V~Z{u$W|AHgSk3wt>*iN|L%_>E*h?T zwKGLK!-nU^(|({Q{pDG1K8VRrhW}shwgIUzYGGyerUH|iyVxkkGrm}hKwt}AeK4Qz z5Q5C18)X4#rt%9=I;qer8Pq6i1DS3R!^*9U)+>iKY-e$v9HIL;vh|CdB4%Nf)s<{P z!*{&s9P1U3{FnL)bjf!i?4C(~4%)& zA-m`i%^MBggCoPrwyMw2O}*!-Fq?+z=;b<2y=T)6QIBKqDocV@texBTmNw4p>y;9( zoSfe3KPK80CYU{u6pXt$K-poCuu)qw2SUSM8}x$ytL{p5g1-sFbJ^4*s$bdTEjp{rS3^|5LMTsENZOk=DzH~@K%0huv z2KVbtC1HWfMgyc8IY9zPJGA}8eh5P9rO2zzdsKISW>Z?IqfNI4?S_O^y`>1;LenvjjzkC{I&fw+P+;Md zrTxBtb?eVe+S{X%eDVL^L-*MBzYiDNO{?myGytt{DOo)QUYOXuOP{I>%IV&_yN(^=%K&Jsvj@gpXK#IV`t7QR9uj@y#z9RnQ6|3au&68_!;g z#_t9?@wUQmmvIBZ;^JakzIAzT!gq1fB_dB2H(c04l6R3$l73xptexTVC5Mo?c0O=9 zFTi*d^h?1#{%e|2Vrxq5O_&6Q92A633_u3Wg>XSLQO`H*Oup}&`g0A2rqA%u9wW#W z>{AF!v#D|9ytG6^YP!M-w9Eb5{plpVXU!$^Z&nyQez*{c3LP5G`jd9n(yaYu^sh)> zFcHi+F@33^Em@CtSZmZbeyAO- z`V6cwrGhW8ENjXQJIn`ovGWInn>?9{lT~%jPa4+lUiai%iuFs}Gt}ONYKy~mWb#f^ zzxCDaK@;|}vHETK4gY*YL=XRenG}*enc|JVwpUOYUF7o8s31*TiTX9EeC~M2%(Yb| z?8nxwo)43~mD(bkHRY^1R>d^)?2RTqcEL*+o_Ykl7LNSu(hWfzdOv=!AZ zth8zgTaKOW$r*dE4~xd+B2WExZ%Zeuus#!=76g+}7-|#JJsXIAnw15pK>Bb3ZeRFkyIbU&GQRo%#xI!S(A}IBAI=3yQK~8sYH07mOQ*W z#Kk=ox@t$xPTM7N%7Rw>+ryc40hR$^-B3%Rju}?%HoI?D`ofx3vEwX_HWf8C;BkT> z{XCPWsmjf2MPfi5Llw0g`e%!Bstyu^ZTNhlw+Eotuim<&!mKk9yAZL4ZSzNoD|~rQ zEpn)sc1khs*>I?-!bQGcBDrDpa`^((MC1~FzKB2eNp=Y3(fC}5y z2wIpRK`Zoe9gkzt?X93!VVD?hbCZ2;$cp_>GAp|tHdWx=oQr8_GQic`cwbv)TsZWD zA9kH)$dS~3$4(L zdhIAQb06r=4xghl!m~mHl38&+Kw zI5yIx%k(@Ds~PD^>{d~o$#h{@`W(@wK|iQkh>|$?Dht+WpiSD^(O9c z0fA|}iMAQHXSQDqo0S@Uyx%QtVLh@YC3yJbx;twbcwn>fLd4wSl%?vLX)RI8^~OIY zNJ)O_bXT@gfBMj9*`cORSMYtSic9^)_gIuB9}_*CtF-2PF*o|Gd&^z|tX56c#M!ZW zCbl?!_79p!fnF^wlXf)?)GB3jZn_hVu#IM-DB9l+Nf|CrBKUGs<2>QGyjMVw!DYT52`hQOeG^WsjTBPngtv>d(| zZQdNmUUq?SE|^S&0uekb$S(cHckC9inQ5u*NX?UK z4MxJzKq6XLWG=tnOPe{g>r8jn&pdf#wQ{s!9FYS*gHu}SbDHTKPcB>;HpqCIhfcDv z_iW2DK@_m}sbs{z`)e6z={E@8NmuF-O~!$DHCB6+sJyA_`xBdOHv{&ko(v21KR8QX zpDw0*>Zrau?a&h7^F?|0zG1zX*Nr2{7DBAXHQ?tW+oO?Yd1Vwa1o{&E)5uL@ozdoD zY34RA0)N(VliYYf;2d4pD2%$hj$Phx?m^yd&Px_(D~FK7tF#KeUOaXsj6jt}57b)! z7fY}Hq;;2yyyQA*W7cYIUUyixhMzxQ_8J!VqyHqcW_i_N%|I6tL2btz$t04CQl+AJ zy7ZRc|Ggic0W!V1@)oY!wuE;~k*yc`Uo_`Oe#(f?BiUWYsfdt_*&d4Ko~q2R7awcG zhM=XRNAgA`_2BJceV`V4OFav(Kl)rClZ*KEM$SbT2nph#a9*GLnXIys_U=>OLYC_V z1-Egv4YadyCD>Z-J|h3(-E=?ruTu=>Ze*_S+$d&r98?L%4!RNO0waNp|?S74p5 z!n(8-BHa_rbWY{9v~HNR!(QfJCJKG8E%>at+Zt|Cb0X5)jIIY;*c8c6>s}wz_R7rP z-*XT@wOhHgi4R|~%Y`Y&oUob2+K(MrmUrz+6#H!M&jH+xeFRa;Eo#HAj;MMY1){&M z^xz`b!78J;jPc`Nex0jj@w7+2MU8Nx>*mZtyf&g?_BDP?cclt9qi30n=oGq$Yl26h z!DxwLJRG{H-QP!pnOCiT4WPbBC#;ytr|V~T%ZjQ7;~xIthj3CtqiQPeAZC}G{+bnP z(1eds_}pVbcp#r*9jxX9&`T(?7e;c*A6b)rgq!1oroTzr-{WH_o}Y76{mteX}r9qaoUB$DBq~ zti=oSm%|TBHh0RssRp}%MaQFiS`9~@{uauL3-&;TJ@oVNqf*!55h{>)9B1&^RnRZz z^iCU^SBk@nn(~+PMs^mDCbqN>DwWg5^}J-HxWcB=AA9q`cx;#8Qi%7kp}b?wn}Nlf zRenB>=S7cKM<0XELdRbg{iEXLqibaSs^+M)tZAdOS)ajx$bI#B(9$E#X&=}T{UIxM z9aR;Spk0ey;|(~baWI{)HoXPy9T=YcKP3OhGbnS2k~K1I_hi`elja8B>yHcn?PCO$ z*5LJmi?qI8scD!Y9Y%blb)K42gd05mSV-&-X8?CZAYiAcuNsPO1^N-^`sYML_dgQU z6M2aTsrVgk5=K0JSAUIJ5T?{mLPZHSBIk-|NWwI1ek4(t5KOx z-s~Ov?-!N_1}8m|mYLytX}$il=E`#=av4mdfs$|eb*ha!3kQ85o*A~cWBXA!3~&gP zlH9CvM$tJ>CP%)vzkpTLc@pM1VNJGIfhXeKNkA?)-@AuZUG2H+ebQj#_d&-HOV1v4 zft`>xwAyrk+;wP=-CON!K6H5ILzi3kVM{F9(ke`aO;w|6r`;wsPaRyaNHG)MS&Bf(k)*))o0ve#kljzQD%Md*R{1A5bAd! z%H@OKzgU~2ox;O}^iEZWCmF6$ol~|%u}vX(Hv;*Ps5#>15jL*D?z}$$7xu( zQ*(i%+%hx=BH%dX%7Nwp2ZFh9V@isIdVYL=fAY`$cyM!bzhC3Jo>x2-w(V0eHCDG_ zsPqA(VH@=tE7q<*JVl<;h_De z&{=%6!1uzUOk<<6ckqwmj;Sur`|)H0?;QPCPAt-?N`C8FW!vZzhL#KN<}WC9DQ-c` zOoxYYgw!tj`{o+#_(2}Fc!|b)|QWFgk1X;*qPi{+@0wDR4qK1<5DYS z^?iKqSNFysIf-rm9BpZ^NS!lv@8Ph~^%&z}1G5jTuE2#23t2l}f^mnNS(1^IX#02W z15fRGDl&Ulj@%)`$MuA6PWHHb$46ze8Z1=NZw6(PY~QMF1-c_FMabU|0K3)U>ND`m zfT2sT=K0ynQ&&E(epVdZoCR`dm(ymyTeyxp&now%u|(w#T7Y$5C!AkZKih%~8~#X8 zwrHZv)yck9T9C}quf-6eu8w*Qo;LGj8=ZuuYPd7KkPg&k;_50BxAfnU7kaOA=Z4f2 z{l9MLTgUz)6@!icp~U}4j9kIVa_Y1MN{vYnU7nIt=9J_4@n&A zcne+Ut#gjX?rT{L4?eBTYQ-KA8cvo%$XkPZXQ4G zJAU*97;f*s8SL$2k@YtxBaY}|o9?fq&WDZqX1#bw47-cE_k0=@Aytj-i8T~yeikvm z5u&U6EC+Z7FGBsCO|$&ClTtW9E8NP&tL=rqoK^ z3TBu3C`|%~ltXl;`(xz|x^;bg_&A9$KVSY4Fiu1$;hogE=hR3DM*2?6q12rmcelzi zMCN1!cA$p?6o1Jiy(pZ-y8mo=lAS6wlzZT=eEsL_2KRcXY|SX`sW}rT3r*R8%wcts z!3up0J^e3-tP(8@zcIc%0Iq2kcrdjQqZTVoC(s)IK}PUn=-qBE$c?Y= z_1Kl8aQb?T-FzYJ^GT{Qz{n-s`pat6I1TW3R?vAOu5k~(rSX3`uEsuDp^VI>;Le$U zXV}mm0TZ>p^2e9+KP7#RzP}_kgmOv0(r~l+V`9W&sdCrjy%#;u+E=r@(7;_M_8P78feIlUiTgs!h)r$W8NUVd8>9?G!tZ*{^i95zg zO8gkX>@P^( zc%9U`U96BYW_^+bth{oKGS+6PI?KoDTpZKQR6iVa#!mmWvBt-43+%S%f#=;#s9aFD zHtTtoXD;9#Q;^(Ssq)-ph*f#8=n76$th?yP$`A6}q$VZe7YJPr-l>7yGr*~;7nQ;xbZ{ZcCurEXV;{Gez#j12UqI~8dQIhAffrv6kU4pp*Go&<^J z7ICOKsB89F$P3Bw;-_(N8&=xX^BG(7$S$`B0JsgtW?Q2m_YO_nQ8?z(#Dysrqm7Kf z_(b5%S^u+qxMk5ARpBh9UT3lmDRzH7JQVLWuA^ogMd+wDQ*>|=IuV}~>Pjf&jn2~L z(~;67kbdGhlP|y%=3xwn@F+>;iPazN67I?>sB8b{Z=<~20@5%yRco53l?MnM!5E{% z%{Z6QSZP^MRAv8;OEfey+2Ukr4~P3a$rbhm5h;{ci4l0AXK4If?I{=0YZpplsH(8c z-`17~Fh23LXBEEMNCEffDRQP^m^UD59nrb97tyRT*ru+1XF+)rrm2nvPGH7?+YoakTDQDUL3p~k})4X#fL#d&# zlv6`7J|Ee;{ZIRPU9HsEHPaSiLHsYhP{znk4|@B?tMD(vyNsdppf0W5YxW65GtswV z%0j}KG#Ir0Zu9Tjn^8;S)`$HcEsz7LjRzAn=XBTahl6qKwstD*!j?vE%Ytn~DmM$r z_1U&7o2xeZ7=`0Oa#^ILTYYaDg46l{G zYx*(gn##GOkf|s_07=w@cnTd4L7(ok2a^9DlpyY(>b8cTyUnb_gvG&tRx9z1HFwR! zP;;$Q23zITOM^-dx=nj=&<6))O7p^ZjJ=?NnXMmsQQ$c@oAyo^sHK?E@Q#zb8xUQv zy|0@cA$)-7DSz0u=Kxq>9_D$9=+tFNQb*mz(Wt?hh0w!ZocjP6)i*fH7tQ^V{a z-*b4g#2n}#(J5@R35M>@9Az`^D>`_*Loyc+R{O_{u1DnjpPslUy2)2yOn*+b?T3Qd ze=}6^-!6uqv)roqq@eB)PQ4fKGR&uV|8Gyue@7miBu(X@j%3@Yi8rOWh+9rsbw3c` zXExMv-`6BxYL74lv+pzLX$$tBUh}OHgA9g7V2UX-#@H(HV%&RK(Au_R>i6B7a*kG? zh3VyH2Osoyg6M9WT4Yq@ZccDqp!Vzf9KV@CV)`R;b2=pSwXGKdH^C zZaU?U+O(V9oM5x1{ZyB>?o1+tUkOJwW>|PbwJvmR4s{bL-9W~~hD<@yYBxq6in@%0 z%jx|#s#bGW+zX{rELf2HtvysV3x-$u%uG-Lh5w*VoT)mbnj<)abn0ze^3D zoIZ3@Zrh9gUhA@@__A^1$!E@-i%TQB$VK$O7Kfr=e=3%^i|OPk3hmwQ7Yl)rRu`40 zhJ}N*xGsVtVL9?>-K_Dsi4}+>K4(<5@G6VP6~JG`pS7YM8cFeL1Jl2W=4+w-v6f$9 zP2*>8Aa+M!b%<%>79B`mo61<_*HGntWJKTJg3q#VjZIKs~OKHPmm{TWvm_i`}I^8MBUkC)a3H zJ$$GeA!FNMuBJBc99=sZ4lW<+Ry#>MgQ;xAb4w3gnCmkhQZ;4bOi>&#-A2?YTs%q9 z%Fd3T1Wwu{fHuia`Sfy`K-tklV2oq@1^8bkh$~^D29)f_(zD;hc%eSO#s8q>lx?=#L$6r?g&wCj24`GR=u`MG#$ZZ{Q z?JRG1b;WeY=2Yij1W>}gkZo0qihG_EBH%}(S1%Cz!e+tJEy;guMsZ*b?3H$Vs+9K! zY&=-`ZBw!I*~xC&zZJA=tOR}RVi5>7zv-_N{S-vfY!bz^# zXARn27BqlsE4GJml59;NLzMPn$jnJuP65?~HZ@43S_tW?oB64rCKd@DGx`T~IW2jB z!&=sudoqa2YQh7WCo`1@^@7Y`h^pLFc3EkN^4b$jcgn^~Y`BZ3e*rkzJk#uu{&+Q& zNCv*l&Ut+o%6jW+E+_aG8>1O~_kmPWh6@MUk?UY3$2Fab%v$>7T`E*TB2Q04yh|bx z#r$7>FyNrz z{GIjpN3o0n+?K=dURjDdf zmBlo(+1KKmG7YGWv%$0Z@4x51K|$xu$@&PSx3Ly#U#YvzZ-lV)^e|YoN+LqehP92v z`H$ZXhpaxD?^{nFYrR>TzVt^N3KL!{1Xb6<{<8SSLPgF{hR2b?XaQt5$U`E=X#}a# zco0PQ85#9+=|Pd9#^>AA;YPm=ZyLzuQV<6{EHr1zxVas=NN|>?>3D_DE|Yt2BSi~e z*e*$9Njd`8b~!JSmPIPIT$F-l&Ie}nH^eZ}lrv|RdU?hz@kwp!RuIqig&FyBGSe(_ zdxvFD`0j%Tyfg$6P2{*QpW1e)9Q-jiSKh91 z=+8Bf8>3<1U$__fK3JGXo4@3emTB|I&P~KsJH1lGbfqp~MW(n*C}Tf{XKgS~@u4-5*1s z>3`m~*iHU2#EQ?eyS`7%dK@TUocOJOKTUh1L??x)YQ+*+Cljs)2kH1=b!KqmTM5nK zLT1`(IVXnZU>Lh_n{A%pDIH8jgJIf*Gpr9mL+N|sULwzZ<3wr$DeET)V3;)B7k)5* zFg1cHf6~*wk=`&~q^yPJXINCAwMj8#JnM0|i~3w6sCZFvF9Ft(F~Ek zx6W6;ErrpKLs4naEr~3ZsON)Em=x?3O`M|jL~}R2C3VH6=mw?bb;rlmHs_G;`61}2 zGu!HY;k?%HpxVy?$=X=>k|-JjVJw6JL~0b}U?hPT=^vcrO`pvKUKZez9xU61-vBfd zeG062yHKyFcez=&3;8-Q>Q&5=Dj$f^iuWY{9v_Xkn{tk#ZJ+DlBbCZ%9^^p3iJ}^+ z=V(T5M*jml#Ydm?EQN%YSEE{tVg`n9S~5buAGO#BdSa(r4K>-2qN5Tj`l zkQdKoh;@y(mR zSxI_+r{#{61`-^J(Kt`Z)IW!7nIi&^I*lG+zrcHh*~30higJ$2u^LYlNjVBEv?e^y z8UI=?66jqPHp&N{jx@-gU#w~y+aUKBj`ijt+`}DD&4d+e+6;@o6{ucCTCg5G_%gvV z%ezcLRC~I$pvyz<=i#Z43*E>cOBs`^H>F@Z1bOZ3FHKF?(blXLSrSjT%$)7JLcw3( zz&m25hjD=8ziyPfekCJep#pLj{L+pFs@b%sRkut_gUcz&Z>qUNSPXdvTQ>{Vrgz7| zXki+LdUKw#j*Ot&)OPV?0T|GtFlW092LrbqeiQ%eU2U+loGbX$G~ERg!CC#oFsL6wcr34gNmddO! z{fPbhfXVmG0#PAS>l=`J@?9*Q6VA77*TqTNCaP+Yv-d(AnsM=A&FW4!#=>8vte3vY zCMZPRAKvd5LBrc$R(z~WF!+*P=n}?}0XlsAzP8E;aQk%jse(yBe;*Gl(-E$HvoHY^ z-|8;^z<@0I;77Iz&9jak`X>?Y{l7)Xn)6@F-$jUK3DI_)t}h;$+xN+b5FHB~-^_sy zCdl|oJs@o=yH@->bzblAJ9cT$PZfWcdtZ>#m}S=NthZl?0TO&a;7#s}deujHVbm^uIEBETyQ@Ec$c)HMhw8kA&qQ?(DxM+Xu9LANLLDx=&E}u>J z{B_1eXCQOvlF2OS+@y1%%E=%cMNqW~W!#QY|cCG1gQt^Ef2YJ;hk3Xu{; zgW$*JW|Xu7a=SbwCo3HXX&aO962;|>#|^?-tfwunP@dE*IJ(i-?-YC+S2kIx)U;$=xl$V%l<}t=;=dzm zeagjKsMqd84|8-FxKsa}~kq{beb#=~+!us1kh|D4_ zR&D@Ft#2*9c6K{F_IjD({Ay`HMl%*RNUUrfOaI3?Z-eQEN-$6iua;jEh$a^$IS$RC z9-AO0>J;0}O=UM@6QsPoQ|@m4iTR-bqQVXSx}rxc#NPS+;#WDp&OmBOdPnG^wJ=KM zWbwxF2$A{HS|R>WsWlu#2hveR8tvRyH9DO%JPo;RadmOC`4)7m=&G8FG^}B=ePAKx zn9i=@k9s3JZy9}c7%JUB^Fzdojf?f>{SkHhsaNuP@m7VFCeGohDI(lnA54;mYxp7r z#i7^`y;5Vps3@tXikivZwZ94tB@~5yx;6GZ$1op9VEG&%P)*_83o>R8mm8_0ZBZL6 zW(jHG>*5?;o07Et zB5^-Q-f*=asp>Io=85DABIN$6rH~&CMqv6m*s^rZn?Wv5aRD`}u#JiQiyQ3`^h%9` zBAHz@&TqF}FEfmw`LH?EU$WW~vZ$3*XF1#EzqIN2pgdQLnyl@oD)p7_VXF}}ofsb} z?`Ot0v2RM@u0RPeu=rc)K047*oa=er+$A`)8dcN8#5#)JqiPB`H*1bXnq&!{qUoei zIC(0XS%321WL*T z*(Foe0}*ag+UgswXlKkoCa#Teu0rGDR<%Dc`S#=h#wFlT))Mq&_kv$9uJKNwFq;+} zmC#|WeVh<|V6S^wMCPo^zCWVg<*7KZp5NlxcYiqB?O%HG?Saf2TIs?zbtpxwIA7yS z+-6jji6wa5+RQt+vf-u6#PwKKN&SxLbThD7q!O=F{9fa0Pf^*c!Dfcl>-xFda6(c* zB*(3fnc6djGaVr3)>RRQS09&eGUGs8ZM=x&OelMY{MA&go5gv|hs&b!Rz)J>m~ofT zgu45O@;2e%kCpTSqTDFh$C}d9ngt-^BoF-nUIB3^OCounqkEMeWu+PE|6!D~PA@dC zNm+pcV?`(Zt3#u)eP#L1pC;2ekc(#p_FBG|p_!t1sGL>POy|q2w7G$xE?jC_x0|;2 z+eHW9_*05Mx&&8CJEX@PK0cAE>>o{zDYda#CsuUGFLeC2So^dUu{&kUffJuw zkbr$q&D-&}AN$nF8+vas%|$-!o49K!T9}jZ^~4}Hjgw&~QhE|aj+k$lHHq&qNg7qP zu;}mQ5gmS8_ORI*rIAH530d(I7HicjGk@&=7Cb9`n!V}Jl*3*`w^E|PCWOw(=Pd4G_G6R#NoIW|=@gvX@8fr+yyIa)B`wfxCEwn`m>u1kh_)Bs zMR_u3I?WAikCv!>dx`s#&ReQJ+=)FNkGG*1e2nc+JjRB06Pu%xeH8UI*Pz-zUG1Nk zN#(_KgsPg^Kz}c*RG1vvL)m5yvYk=$V)`-}4C03;1Dyv`an*kd>`qVlD|x18x!EJ_ z>T=bT5otbCDI3lK1G$iidptEZ4w^ljM*Bj0`2BNl072!=#o|_|@?DI7w+(lBaktn5 zUD!7o_H#6svr`Mb3H>s=7M)CmH4US?#}p)@vI{>+wZ(<)5>na3T)Zg_mBG6usD@t* z8Vw(t8q3qXdH5YqLbN1}!OKK~m-4h*#n-Dj4ZGu7EyHKS_~xRER@PdI!{|*#eCuro zi;E1}yBUrv=+CX)-7DOh=R7NIBdHhJ4IagREVs&bDOI(wFqYMmGOn-8O>GwVNy@0V(EdB3AnT=nxW^*zSxD0E&0sN&*Sc6Zd)vKBIU>!beqZ=cud>i}m^Xf#G zt%_v|0bqmqw8ptkO>fagx7OU6{;v45b{C5}DMG4v{;ky>Fo?|mOokhJ)OsU*< z^w@am7Y(0IpthMJ`bMJ1@qX2aqVg-Ai-Gw(8=UWzD15C-`}HEFNVO8-uY&k+a3d9^{zk&qLRRG>zi&SeTo_#v0Se{lV(Um z6G>$JB=Fbv0LL@cR*F#<$YIwyhtalbq4pUh;Qe|lMQGir*dcbg!h+17!uaeUG24CT z3Ke-5;|?nw+SNZCJ*Vx*ven1X4NGQ35wq1eV3fNGGuHFil+3lai#j>!Q9W$XftnF7 z4Aw+Gc7|)Mv#i}4UYYunXdu0s4>5Jmsh%5zfz)P3W=vyOf5-(5baRcP*CMU?pi9(= zz{+!%@K*U{uQT|t3*+UWF1Tcp()Rg^g+@IKg_B*NNabt5S$g-X7kWI8s6n`VtO0(P zzQ|$k6kw6X1kC{*`$w^>^aj$|T445c--tfa0)8g56qudw+MtzkFHj1Ja-u)X zv?~N?neI73#{7FC?KEhV*6>BigHfq$B4>qxo?Oj&4?G`wBA>1&h(Tg z2P4%qVl~t_I}j>y?hhZieY!1o`(~!OGwDnY`RDZVOwi5(Chr9wizN+#z~8unHdwX- zPt0f4jrFb+S!r7mvF7}Jt=k`X{QcyJ!J@GeKmFLgYtt4d*^v0nB^Iep9^Lcu*aXCM z1u!GUgYUOx4hj@g`}Cwc{AwbHQ>`qxA*Q9!5F_mOp8YtlLiz+kiUK+Iedgie#K)u` zXL6-H4puia!h_K~A2R3X)tBJh`&I2gOM=crb&`&uF7xQfi|$I zU!=*{b2oXC)QuaXy1FR43Fk{2Yv!rs$ciRsa%HU2c@D%BJ=LQdsW`bqdY9VQegjOf z0QMSWhs&rMzpCDQH8hawZJW*8zl-bf43@8dJ0lBDGw5~Y_`RpAm9>$E*cB`Gu02=( zoFdPM+9E3Y`rZUnh+LKZT+)N1AcJ9=c=^i`_5f-V6g3i=D~*dp%zF!;r`L5+pWQ9g zu6;s=h4`H74V>YQEe|0NABm=ER4@@uH&oU*WSiuRT&6|v63j~$-unhke>!dcVQzTu zSr2|SVy7CEC)U3PWivkK12n7jUp~imd zUFgzujCir)>$slSd{341eW#l3!SPZh(ka?$$`in9E#~H*Ea>#g(nhAg(^Bf5iA;17kBPfI+Z3Zs?-7S#bH>#R zBK<`NrTl~N%vvGBEvp;a>~Ur_!|&(IHl%Yn&+XnGni<|(*uLR_jv3d;439c9hPHCi z-6x;$3SKO5Fi@PMncWc`!}_T;9RWTjF+flY<2|J*M>YJGke*+z6~OGj{PtjoYSZU9 z_4M`4JFKTchYEYM^Z7)pe`4-juL#?(`@6>D;a>F|%g$MKN0?~$iq(Dt3!T;g9*iH` zWJrI`V-{y)#c%X{7*|Sd-CbvWHQY-gwC5!bs3{AMqpf>Rd`+-l!KR6L}$@Thd zzqYcvkBXo~0~NThPVBw>Am+9b9aJW!V1cu}h6&@5+gm|C4Xi;wqMRj9;c&z}?hfyv zXs?v7A5V2O`n=u`xoDQVIKLTE?$`h1-G{5?72_GDP9p%q!D^A=w(5UJS}DnDr~77f zPc{+T8@RG2eX&#QOJO@c4up#(H<%4LPvjJFA(dbqHq~y-F40Yfm;o9!>G=>ncq2xD zZL`9~OT$(rUr*A_*j0E`=9~%dk3bupFfyfDu1Tr9y+1RBW*?g{8IeXoFldr5?@*}@ zrQWRP&zlnd+))4(dtU9aJ~|l z!GA}LG@Gh-26oeE?bmWB z#nVO|;*#HIpGDQE@gEYcYcRI-qv{HHeSvMN?PNuz9Sb};lK zZL!u^v+DUan!2+QUALk0$;m_u1kj>${&Xf`?jza#YXL@F2S zezrm4Ct;Dc(T9Idq^P^bn3-IyhFS|u`*vvhT9?Mi*ENVg`GxCbI6$jLJU zFhb`O{fDs!rrB+U34lmMKs-dZTWIhtZ^oRsr`jL=n*xHsg>)vR;aQw<5Ss9veU*C-Sf^{K+`Sp+?pu_VIh1f zyP`NSh_R*30Vpboz~ZC^#$rQ5V8r|Xj?|`qsG+(7+4#Q1r_QVRJ&Bn7ZN#fm-l=*a522c7sX;uzpkKdCul6F9-@C^)Y4#JS8MDv zp`@$I;zFQkHLRX$4Y79i{1{=L%G3|n-2YtN=)-1#{&f8Bh!>}t_Fgm+@l#kr34Dul z8bR_UK7otL4HK`S=|M|>K&#CHJX7V)@NO<0OieeFA^~lIyeA($_$teBD1OymPV0$e z2$?*0^eFr@=T<{T|ys^a;(we z7w<}ieNiDCObxia;UJ4QK%Dde;zazbI-s+R?Z@VbSAsB&gl>4wPeFEn`B0g$SMllP zB3)5S{D!E!=Sj(ws;$RyE#(c#qJ%5iS2|n?1u}NpVm0)WD@IZ>~ij9<8%WsTHql$KK?N$XV52o9~;IGtVHG`!;9Hn@#l(S0(eUWm!%CAR7b>?H8)d#?ec6wkNo*Fm43a3Q>E&uLhNYju~)e#hoJu^dis1xqA4v{SzBUi znvnt^`+bIhqhDqwhK-vwVwxBgEWZ*~-nHq*HQh6LEsTV@eA$^I`osr`#%T_sl039w z%&t_*S+j}3r`k9hfyMh66_g*cVaa}G@q1#BSc#iti$6>vs4lvdQvf4{0K?%*+b*EhffYUCMLdO1qh*@8fq3_9bd%8@7Ly*^lN?BAYCY=gjJG)?qzt79%ZffxxBr$$ZZ;pEd-B zKGT5~Arx-vrt(2IwIVN~d#iu=tc)sTE}N5S5K%1H7`-7B3B{LrgBupJH?Oe(&w*2o zI7ONjb+H<6P~2Esbhk2U+-z)!Kx-FU=9s#WzplGye;~k*0=J)i*uUc^U7UU4#iC3Y z8PnepsCi}F%;0+Kcvx^%pF_*rNjVM8^E{9Nm0yP^wc`c;bd@bAl zk;%P>v<#QIdAJj$u_rzl5zPxzV6K_tNCo!XO8uC^IM^)IO}4_OZ(xXVf%xJZ5=sA~ zsP2MZlx!DI4NN%x&Q(Df_+ylq0GF2S8z|!68}0l{+1Fdo`iXM&`6D%>6l_HjGa7_5 z)alI%?4HsEaVOlmCfW~LQ75+JpcgyAOunEPa8B!LP<-i8(3LGa6?Og_#6}miT!Qp9 zW>9aUacZZr@Jjr@eixw)D8W;u`i}GFW&cva=o}%a@N68P)n)AaN5i#2+YfPuu;GYv zAizDRqMs&t?Co}wrCbiM-E@sMk-(bg z4_%J=GD&(4%kKW*C;hXmni77+{4-XaC2$&bR(2X^33KEaZHY46NC6HwF9cD3*O$7& z;9!owlK))<3ktNio!1y^h5`ZElT2r#+&<%T#H|jh`&3dk&B&x~Vm1I?t9m3zCey4TityeIVMs@AZa{3-r`dg4a_)1J_m(}cT>#?lz)^&BYoB_#zmfCqJ zFXEyZVrKb8IHDDo>2_M|anaW=!%7klhm&wO?{w*B(Xf2~Aopuys)(JNpSC^Vq=BlI zadG?uJ_MlrjNDF9cIZ?IX}uf$Sm7X*k2`982ScJcSKYu>AOmI25nqq~5Jun0hF31O zPl|IO8h^N=H;h!>FV||-FD>4Oee>c;W%1!91>T-|i3-my{QoQ(^NX;tKpHVHsnDtZ zM-4_Ze>w!_Turhj)o&muUiCzjpU#OSdrdnP`S4%= zgsnCTpIxt+2N40l39V%#=z8IW?BVCtg#2eD7~Q41Fj73s+|8vp=RI1wH-%f{EbFbh z03OHWTM*uOX_^w+x#PFC@;naE?eGXsSD%_fYyUn!&GH7ytu_mN_=mK|foHCa%Zqg{ zI=D$Vx;S&Fr+2* z*_xA@)Z~|z4JdOd40duYFaU4HuCdHn9?Wa?H@0n4X(>iGd13}!$7Q4psXRU|+Lpp% zmz)cXO0(q5S(5|eHZ%jk3rNdEx#>pEqYe>`aqr8gF0rf=Ye`&Nd&%I}DVknqa>`p?c4&*4 z;+iO6Y;V4Exeus0MdKr0pe2yXC?pc)^l>(yFL43x#sXM-M;cN5oQYXJPByQO*qmna zw*;s4nC*--db7upa}L!o{kVH-rlIA$>8DFbJCMyVv>pZsQ(~F%)7c!VN5WQDRYcZ< zkIht<8>(d!yxysnQG0sLe6yGOhRLDQq~)KS%1@wYw_P zP%fi!!xa?S`u4tMtVTC>4{VjUbJXIIy8jx5YMT(?BFeH92W+UeMVll{IvD7mrn#l} z*dFb9@3w*-%#!FXJRCAmB2OE_LEtm|Sm&4T3W_~*D46hfB4_>!$IiD{8T5N+%*KiT zj(jf7{!tb86{QN<%LQH`NSQf`Qt-}Rf&GP;D<|jOJL&yf&f$)c_7pLDPPqdG+x`{w z4tZgv2W=X}4bIVYTZIxvJO8Pdeuk!ssaaS7XP&iEnv`Wfg$2dF9(u=(Jk8A>tLNLeI0NFion2KW7|L1$Gj!F4H7HzeAF`efUNZhlZ7pVerQn(P0Au><(^e>1Rri8<|= zX95tD(69WYMRAt>A#(0aQr4=%=ecXGZ!OeVCSvYefjskt-xwt#^)eXlSz^~u(D3sKNMg{Oj0 z#061A88wcC81<2)^cA)kYiU?7Te%fl``Y-a?Rkqi+r(UQXejBR z53yTI0Y2?_>f6d#szM~M(giDXVi)Z^q~XS>f4cM@42GZb4hI8u$>PbCTgusT4NYmR z2j8?>o5tK9#QXZ4U))!M4`m0KgUgtQ|NM7^PxrjU*UIjL4b~RKH1^kG%5^qu`u-`- zh;rb{N#!037a!`&njQgvO`5WJnEMRIvTk6<4Bg0VzsA+_Hb9&-Kgc@G3W)g4OU## z`vGv%j<@nBA%S<|GpYrqEFb+r)M9w5s$b`U9wTbOg|V=L6wvn3srk$xy(ig(;3p$@ zNND28m}!P}84G7B<2nwy80zCKPk77R+ix!hS{qsrbl*+ioxm3|ou5@zi(hs-R|}W8 zoDA@pR-M#5SRLkF^nnou%=XVbk-7-S zO%|PF=2-`|QG31}UxxzcHpBqKMe#)0{nEW~$aIjoq|f!1pNy>W;E|3PzzJGcK%5dv z)1_dGFAk@AcVm2{aDNO9`l}OmqA{iGBxk9k{_i*P5zk-Wnnl)2gjU}UQ`c!%7 zSCh_PLQL6mld1uCG@NT?6I`PA>29Is`DA@?N!^I|ST0R*0W}9}WXq5y{%s$iSIRjM z2O_yl^C)w<1(}>mC&AEn2Jm?=Trp>p>PI+*)`AEx&{N{_|_nHB+$h$Lz)0}AOUN< zwL|rg+BXL``MCE@kNSv(hz~chyeo z9tSte^Hp~@-2QKKoeue>@!p(rS?}}^AKFlQi74l*JlWQhwnvPgD`_9132F4f0U|wu zqw~5m_n-ON{S)L!!xiX|XT_B4K%9!&Pn*I~F63GO^Rb;9zYFf6G1$FeP95IeKyk;5 zU3)o73yiJS>;Wt?;{u$F?a2eFKXf_RAFX(cB!C1dvB&2#N=^UXz#^{s&}5zh{uMUT zS8NHr6IXqaH=NwCv;sfbeUS;wunoN8h|dewcOq9TuY^oQYL5b~fj@-L3+i`a7+cEJ zPDA7gSWcgDY8+y+rpBrFX%O2kZnsC7k3lCr?IT$d11~3hhal8R1Cz$U;2${n6 zbkGwom}5lRm*(1y4csUP7j~$S5&t4c^gFt!+)Zk@wJSy~g|1GUPIld|<3bZdJ!A52 z5}#T~M=o7>H&&Q+QKgi6!()o;BfAOGk1Wzz*dXK}ra@K&;AHp*0Z!M?ptOT4pXl@!yMxdhgN<9;>(&L9(khsazVW%+M@cwbL&aVmF#jUO|f!v zu4mw`v$o_{-tQP|`*tGW>fScNxxd=t#I_!!U~zrXCp4&fHUF#50P&AbW5A=fR)zJs z^gVMqGbiYqyS>@hwjavF_z5jrdJ0 z7VDY2`8b&JvErR`BRab9DoL#IOT7`@=l@Z3?(s~w{~zx@)O~l5yMu(v9a0F9St#z3 zbI!K0IaJP@ZRVKs_pVSFA#>)IQ`jurXgH^N!^^CkIf3$+|;@Xs=D1l9`@5+Llk!p!INdbk073y5Uy56N~#hVL`;4>$p_o zHN1~Gy8lOX_80!mj!8l#KqgpgOHdIh%Zp%5dL<&vbN9dHIT;iiZg=lq|0FHjm248Y zx`rm3o)b(6vexp@f{lurl%Ro?n6X)cc6vj9NUUe0M)DJ)| zP$Tkv^2P;6hlJI(t!=8;MBEVql6y4fXEJ|y&e*yK$=0j5jf%ZflAE95PY?h_fc!Q- zI!f(Nj*wtwkr|L{qz~+Gs?y-^;*kqlP4~;neerL}V3``1wziL6QsBMkEO>4INQ+!K z5Vt(H9xC5Oe@xGc1_!579y!gi=e;C~b4Co1Amt)DHI7}q<-1TbdoAjAHQTv|sdc%t zxP`dxV0_iB%qg-gD$^(e_pZ(-?~)kRd7p3L++14fA&0PA8Y&m}bqr|~9U#l}Z2Owk zVb;xzGP`eEzF{oDsOk@fZ5}Q&_3wrj4gc7q)U=OCLh@g@_o?0>dl#J0USs04tcIvM z>UGmM^`EE)9yKryG8lrRsQW5H->e)k@yV6eFuX;d%_5}YW94oph(a0V&zfYeHx}GP z9uC*IMSTdD^}rf6EH=jrv76j~mp9E5ci%GOPK+O9+h*}?7BKrpA3-JJN&k*}$wA5G z{=ORB-*Xnv&+m5$Of|D^SGxTs(lt#Ack$|(Oj(EaZ2o$S-2U$|{Eg{CwIOk% zJ0&-#xXr0ny*Kjw{%tL1_uu|IfYXQRUc$zo>u99j|Jcy6Bt5yh`1C3RzzcBahB| zcX#vg95KqZCGTan`N`)*n|VlHav-N?vXZafvkCD+D|;@zB6$i*#lS`#NhyQ3nY474 z<)VKK9+2ms>-<)y`hNS16slY9P@JFDiftOW8QYKvy>4dEJ`G3scj#)lwhuzSjIquB z8LoA8RZZ+?cIBAocKwCJOTxqENsV(^`3m6d&cudePy$BAd&;qvZWC&Naqf43eDXC5 zQAAn0SvwfMWS+#{UH1e{(uLtwt?yRhtAhy~r?l{?YT+rY?)_8bSF9~f%ZxjV?iLl* zmMm$7HhushntUlq=2?}e@4920B$Sp5bI6S4Tar%|t3#R{X@E=Na?<$w^`ffX2Na6vD!Un zviol}ordd?RZ8U6?Ry*_CWUR<^9>>h0q)NMG_LNsh3X(!|4Ci=L1W^e-Dw`f-8agM zW!s?U9rRLN5DdB7_ge83V|kDTpz6*xl{dHzIUNps%xG;yc--umgd*gj<z76vKiPaW27a~HS^T~yrcKGD`RD)M6V*#q>n~r^D<=~;`XroKfpjk5Rb?S8U zLS05nt7&6oeypXo=Pvwc+NvNh7Rn%x@HL3;^NQ+cL(!JK&*ns)XJbw9_6^hq%7d*K z?p{7jL@p%UN?m3&iXsTs*CV?(4KeWljsV?w2=k5zU7wjqB=%E~OKI}+wX_zd6yvq_ z+^l%_Bo&;8`~U;>lsm&pugm-x12%-~rlwO_--6>xrw@I)tiFPaR-i4ZYH?)!!bw}= z<8Q(=+1f$B4IKVj5w)tt+%p=*Z3SqUzRf)=ENrv{H2Sz@A)z2BHGPT?`7>$ICY74`uh9N|b{kAkLKM zHnpxea-ip?gT*5bJsepZe9Yxjl3pEh9&qpR%4y-&jetd5tDW#dJw&phg`O#dv!CSuT!#jzNc#;* z=@y4<>00QhD6Mu{uosfY69Q)PLf)d7-oXt9?QoooAv%8n+7Wv1?_lWI4UIftV3v;f zl;^t!_Ci*F0{E(r*5tgwo!=`}Pgf839?Rj+IiAx7N}bRCDVqqQwm&ob?6=VL^@vg}@$Qz;SjOl}ijJj)%f&I--`$_*goM~M6F}@3!{4S4{TYhHd(TuH~6WU^$_n*4Ea{3=zRT zL_iT2VNeVCIcl!uGKa`BtMq#SCY8eluI``{+*S=9f*g&UhG{K;7&e~gCI>3604r9kThN_;F2&wpMKmq5K7;bE7sr0g;w=QFhq z4*91b^GQwE-4`EXO|xgLGkFKagK}XMUadFT%kO+Czc$6Qb<>o(sUV>`VvwAS*NlFf zs^hs2+9CEN@>{jvL){kX=1iOEw0tB|r5o8(6=c!w<;ajVUn}^B*TbvLzKfjgPdl|A zI7)`)>-|}u7H~Vlc-MCGX?tnLrW7oYa6$YpXy2&T@Afj#3W14I%6Fe!xS3Jh=`>Q1 zm#CijYq|QvQFO)@TkTcBT}Oo$Z4q;a9QAVVC=ixtys?I@i40lot0V1}T^yg{Rky^w zE<^Ynm$93gJnvv38t5zYmhy_|#IHQS^D=DMaA57G;{-O!LVf6toIyjLDYB6t<`}lm z&2ylZvS&H#zUz@+`pMxZ(5fnZoUDc|7HW%8^y2HA20@p-S8f9|FP;YL5CJlQUmyna=1VD=5Ea+y7N(lV_lP3xXG2ou z^QC~lcJPZ{ENx*?0_D<#PgP+u#-Rz+!K*5?0x0upbf`(v`hG2QnfCqyt7#GLnX9JA z;S?(~x}(dk7qL@m|1sG4o1IA#KOrVuH7BNxHjN~+Zv=G{7Slmo*ahedluzoVYq z#>pkTkkd((ifg2;ROxksWx`*le^cPn7R+rC2 zj(=pTTiq2GrKk{adj2@zp(uNrI=C_+u>cKmQe*)bvHn%nQ_g@P5+&{E2 zrc;YAZEH=-?C_I38`pV%Q}*z_7_GUv=b_2FNHV0C9rnHbNdVkn$dX%sG?6iwtC{o+ z-{3U?I9b(yr&=jJ>;QqK5uDG#Z_H@a_-2@D=XvQWNKgL13Z(V94>F-jEh3Fomj`J9 z+AI$w_eMzn=_orE90X@zx4z2$9o&H{Z=c-_J14pu7p;CDgStBV3@wDusQ9daP>{vL zZ4axf*NLx6kn|S3_gn!k!&jeNV|S8}1uz=Nb7{ zbE?35!8WVC*=K)PPbmM>+^(&k_mnj6DK>8iMv(03^R9x{uBx&5MZksi>^#%VAO??I zBEF;OxI6=NP82b3yO}9^&vKfojI*GH;&*>GlbUHcpWvqy@R(@c)HHw@H(f8i_uO@}k4f3W<6(W>s+F z+#1T{>C6a~Vx;@wRDul|sn1Zs_U{7>r){*)G02ZOKG0&PwBB6XN!OU=K^T_!#y?v> z^K-M};Zc`nwrh8K12pq)Jgx+t_fP63?1kg&W0Mjd+KSoPPbF{FnRRtjfw!A>bQ_`K=GZ;OwSj3;blnYc5cfnu6E zICy<=-hg7STens7skE1x@+7n|!sE#%|BI#_8(p^^Vpn7^-07p?Xyah_ zcFyBPcIyt(95uI3X2ahn6ONnO4rI-bSuU?$JYYNdi?5$U<(>S-mw%@i42Ju((C)d& zqIIHMAq9ks96%j1;`(^b;$qJRYXqAbOYto=BPfOdvWeC-P{Uhq#2c}8LU(VHa?VM* z$Z;`@ZJ+FLj$Grj4}GblOHo`{#KE_EqxaQwH$){0GF8s5i8_;$ZL&v z<`NBk=~$*sz=QGX49rz)i}A$p-p`dau|DitrDAo%$p_qp3{Q;+f7`UDEn;{npQ@u4 z&4Z6(!e8s^p9J$H#nCeKm@xMjq|1BHhIL24F`gT60XuO9TR5YY(ABfr_ zKbQ{d&6{hl=r!ODlb`m3GYx())pWV!SY0uh;Ekxa2*1qKUHh09pC1l@Pf9nt-*8w4 z)!60Q9FpY?>Vv^~+7wm!d+>dB#PZpKGGd6uAY6hfWVA$Gz$8&aD?Wdrj>kevDthO8 zix7AEL@7&(L;Yaju@!4dJU{ymLCu#;tp+)O1K`(m9R-(|;_a0@08U42)g7xt=a$C8 z56Yll^k9&OvA)D>1J56Rb!yt_@C#tnR&-dpE)TAXGyCVi?|`CHz;EeW7#F+R%;2;g zf{&logR-aDjplk0MOB0a)a))kf-}S`1IyovCb5C2b#NJxa2vz=Or-VtTof@+nu8i_ z^Y6j)t*KSE4_1H3>5if}8L{xtPw}7@G5T-szbc1xZBK;z&X>mom?WLy-bMEEp!m>5 zvj!|V5jZ(3j)eOs^`uBJYXOBH2!z!1*sw!YgfmSU(`ABDY)lGYBWw;FT0V!*jaoDq z`edc&A0+t1N+7?rT*B?}@BTYr#)DTPW0Zn?7gko>j*|Y#P~H2V3VzNusBE4*c%Nkj zTaBSt?jpqG@%T{Ju^Dv_v#1xtK3K%O8_YyNu_=xsN2aY3W=f}vlVu;p`hZu28bpWU zo6M#YdF4tm^Y}+AzdsQ3#57Fack%e)Akp@(W4wv_y_X*rbpV5%d%7zvQM!3Bfs`X|`* zuf?x%(6xdyckL_|njA^%t>ka+YP*HfDYDGDI;*DzC zT=D57alqKrQf0{)%I#2$Da{nv)vbB(iT6A?T}mlon-<+auDZFePD``W+r3f#`$+d# zxDt|G*2G6%M39aWQ#E)fBJUC+BP8bEwll!E#HFWbWBv3?f)g8tutRW901OK zPwG%tu%PT#p=p`+4UvqdQKd%V8LshsGtXHFM$3y$vdA11-8pqx_kf!p3rFDK;#!O= zkdi?pd%!;hwZ2sIXdgE~G_e_LB zfJBNnHu7Ouhc`$?L`KilrWyGczaA@6-jv?CSCF8t%UCmX*()R*k0Q-mxRs?sz!|@P zCyV_C{+SDaPjG3Qz6r4jtaY%d?<7tPUes@^yui2)!dcWxHOsEw`qiOikF3|#?T^>N z8GqH0pROUeVWann0^M*&srZLNc_8#}{*4xSl?X4j&(#B6xvxu5KUa!hi+@naO9zj9 zN@2?O+Uw+J2QkBP2%(h&KUNK!)b~0w2luM83*y@B4obvp*l~!M=xIC1n@r4TX#c{9 zsUlxjo-DPmiyPqC_%{iwEv~{}&&4oZ`3C3uzqIX?*uY9wT-u+k(60oKEQZmf6@2Ok zbhoQohl(0n1vVB<+of!+prBPh9kCf>3}Au6uV7lz{E0oEJ*?95Dy-7l=w24L++U7@ zmxKVzil&EmD)<3=oa}Xnh>vJ!pXkYUrF!==-ig+7+Ui1CelW&{Cjhzi;L8Q~mLtgW zD!~J*aL|*cs#4S<;5ysH$t6E6C@iz6k?BbzYI%XzZNPi@N&f77(lTjRqYm-CgQ}87be@ycW|i2#`ueKw_^~>=2mJxWWE=- z4W3;P8xRTVH^G>BUM(@=k5qtKn8FDKfv}p^BLGXtnY8~S-e&2nL^n;_SMz7mb9!jU zDr_G#UgHQr@ovN7#Dhz3Ac#uXsgKBy*FfF`;N4Bcka$+%7YgO+$8f3QkaG7LV*#`Y zel1GvJcYV0e`Me2NzC*L*7H$Twu3-Z1lHK%yQqkaT6#< z`|^dWws;+hc*jVgWv4r;c$8EplR39=autW$R+oT=KEW`xcz70urqf2k)v0^n2VGs= zI6U+>r;-jJ>yKIZ zkvt~cSIR;E{!%cm#l4Dnn2Wq7-WH77Y+otQQvLI28)CR!ZMW2R)9m%1ts_HOaMp6| z*@`a7?hjJ7fb;?WZK!E#Qf-Xg4fNu0A^TjP6q|GKtEK(<2$k6HzE>H(YG z$y#|GogPYgG@sC&{0Mkoy_rH25G0*C578qHiX90S#c8%JJpkOQ5_pCjl=kr96Q$bV zJU^6%^lxh5#sekOL(fzM%n#gqerl#>ZD7BXy{t9M&AMq6p64Dmf>djM(UfmdR;C%L zv3b)BtyJ*xCV6N%FMvS0H*NrWzEA$HjGvB?i9UnDyR1Y)k?$=`>I()&;?B;ro)y@M zObLi9lIw;(_|nxGellJ*!@}A{_N;fnc(Dy)06nK3tPukh*Y4e(e=j1>|ETd@v)v|? z@h7sZZj6^I@LicJU?cBX8a#1w8P01>ecvqe4MrZQNewo%=y=u@40D%hIqP~8EkqPF zdoBR{cb}x~ywJ1$Ts08U^dpHZpxV|4fwCz(;ClH19alm;VCOhI@t$dXJz*7BZQ-d@z>FW6_g;b%2Wpf&tj&>Y;F{=% zxK-*tLgU_ru!YLB`{^wQ%Uz>ZL2t8apslP7M7n}RE1w8UlQB{9JX1}6N=cmh{}bHr ztE&t)$DS8riXD6XlEr^1Swt0Ymyyxs@aqY_>F=SnVw<0cl{5@#4mN;x$538fqqx2R zXLmWQqIatIW6y9~ld=!boac-*!1E(t>v|f!zLrUJYGz#d)`BCY9iDLleX|z#xP?Og z*{gC*b^@w6e{Nv5_cbdMD_^CmLkVbI)B_)ny~`}?MiaZPF2 z6o7iiOCIH%10~)KA|lv0F>&t}SqoOnFg(O=4{^U$BizYj1KUT&u;(U4S%%C7bw~f7 zY2b1eZf9fq*g{QqLvh$lLL-(voIzicdii=#pH=aG()CYiURMyV!AkNXK)q}J6EE2Q zx*G3%Mq7^sX1%3?(td*cwqJQ+Fx-(@9QT0P=*23tKzO2Nmh}|fkFBpeOmL6Q0(JB1*ab;Wz0Ga8oh6^uxhlg3VF(Ry+?g#vKRKWD4DF~rk(ZAL(l}oE^(0Z#I+RG*49bmllmS3l}lNT z-&=;+alFIT816oEG@%H4X?y)bZZZ1sO_Ij~zNrk+)9<9SN71+qDSOiN&O+eKmVBV= z*`Bjz6R=R4@qmy4cM2U&)P|KN`fx00^W_|2L`hp+Q(Gc=V!=NO3ob33sGtx}@Rq%O zsZUXO)q9*~g@O)EM`xZ23P7k+=JjX5ucH_XXh(bP|BrWP&A%tNB>?*;-}wXt+s5Ta z?lXV6wMV#2(Z6|5LkSzCf}QW}-zT5zeIz)|cBT7Ww|1YC)}79+6R7)=ThnoSrzh#q zG~Bv9e=nkjW9`ECmm^icyI04L?02^q_|b{=C2|lstgl(%Rpgwcuv)a+fTWozof#aT z9jLM}4puE->X?7*Xukc37YprX-W(X7KdHu?sL(baj7xfnBibg=E=yC29bw%QG@YB|$_tdeb)W{?VN>#iScJ3D2x7+0=aIYJj6 z*UepF4oRGK{*Psy+eL{z!mGjxDEjrIuDbW0`1XX)yx zPvye*UL*y76Xl1g5!^H^4Cl?JNm;r8JUyI=Gm4HG4E1+nu74^ONVLBx1QCxw=#hwV^( zZu@gz>mMM3Nox6ZudY>Bmj!wy!?uzJDed@sey^Tr4}9*;v08aM9woz%=y&e5v4VVa z6YMI-w851(0#^PzvRwUxse3wQ@M;aOTDbwh?wvMB{rJ6l>zVsJjPK}N9dog*0Jo^P z6Kt7Y$Q`p*ei1}CtomLZ9@)j`M!5CMqgx^ zmiJ^X#3aRr_ggHY2-Z*iM8`qZ4h`@7`nB+9zw*mPD&W%9Me$>?vF-K#T7OeMZDHYM zN>6W<*BgF$?ftUk*g%v~QZ0M>$wHlH=HdeR>_$XLTY1)-piFb7TMS&eS$#5IJeE{c zh%lo?I%!Pln%v9LW@tHZAa|b%HjPyO056#zI!?P%U*xaf zJX)(zls8Z`Qb(FXgBV))3sXAb>f+_xbW!#S5VUR+)vM&*0ThA6*I+#EIUttQOGJ&F-50XtGC3RIUKDoU-DN z?OdOao5ZtjyNn=?E9E>CCHix~;OW`s$qb1c5w%=3WQJ4*(9fo*_)~->PtvI$D$Bp* zYRCEzhL?*h*LwO7zYrBqfLX@DRqegSbD!y7Mq8=CAX(CXM-a!NSLL&=vEGY-|H!oq zEw7yjZ+X*JZpC@q+u9Npd*tTY2!S|1$jupC=d(zewPtwK!KZ+}up%`Zi+1mcpKEK+ zm!AMP<^+Ri(G?QqW1Mp(;KLPA`fDZr_nny*4+XG;-kQ<;T|UA^LqqH0%_$zfR_gn`d-aP$ans*P zX|OS!HNd#bpHl~`kl(5-u6?K9tjGD(@SGY=f~Y$4CFz)}KD`Fj1t|yzE6j=$snmSz zh4Z&-A#P-9cJ!ooW9vRgwzVV&FuT47MAbrCJhYWP)=z8^s`{4GifMVN5K=<5g)&Ga z<9{*J25Y)4rOxPz1OeVO@ZYgr2fwu)_sU$DDUW}?1*gUE8|&(bto%P#En+!MGu4%o zy#%1VHVB%)83&_-W-P?BJ%LofC>ZEXKZ#mn*pJ%PJ)IQwhblL%^ND1M%So3h$M*rR z@&YoPHkY=555gFT;kTJfUNplU{8dNAAT^(Rzw+lmBIGrxdJ{dTBreItFyX9!Ex`Ip z(kgRQJ(bn@^Aw*&OiVzdI+*{8g$3MBYWb^|K=1Bi&oo=feJa6f`ujwtJJxe!(Y^(_ z-lVizW9aF2G_A8E_t6$^8zire8PYD1C_|pQ%d1I~^Elnjogu<@V*%_i2o1WgHCnpZ zcXMh@mX+Aq5>srhXx%o3+mp%}m|1Fgnf$YK05iQwX`5V~H;+JEJD#@?VV|eCZ}teh?+ZqL@ZJ zncCnqv$T=QWUJ))nfeG~cw@&epzNuTh+|nBfbEm7-dQnHA*v;1(+WzAeGk^<2=&gMsKy1XBKlU$^7J9Mz<0g3p8_9GK{he(F1h{EknF*xSST88)G_fdn6vJVKfs?a z2I5@ch^Q5-cn{v^nvc8jVP*!TqKsQOSpU1%Gr%h+8H(Rf9-~M*>c+vG(TaMRe01~YYV zW)ei7N%<-llMk}0ReOjb%f{HkFAGR6D-4fOgEihi1dIA16YX$1OZq@26%i>Gou%7H zwdBf1Pr9ZY=fPl<8-Q*zo%AWM;?}S+2#Qncu@2tOZVNtv5nS1l;K2#vtmcaaP^E=H z?G#@O+)wqpc1)skSlVrkm&nvlF_X7j@|*X$U!uNrbmi^nnib25MO&AOahYhsmvQff zQlGtM{M(Q=`j7FT!_Ah_%z5z+-ZrnG1K^W=dknKZoa)2nCL!K)k}f|d`PcNIV%L1c z9{Lwq{8ujRK4QM{pRm2W@=SwBrh62H?idSeGHfA zLO|y;_U=dTbxdwopM~Xk{OZ&kMKMj!qA2(PYz_zYGL1s3SAhL_sgJb3?lY6V1VP5d zNG$uZ7kT56y@fz&lmwLBlY})G2H%lakBJe1oQEPdc8<>+ZSM&og^l&;E{7t*GWqq} z;2Kp!-zq#$_tVIN6+vDVLw<{~15~Ts>av>KLr%s&p&eAu!3UP+S)xC#bQV=;IS7gdzB}@=f_808PS9laIu6xbm9PVkS~sTU)4Nyru1Y8X#WeuU^DA-p9CEh zn!ydk9nSl5ff%~7y#<8gGM9b6P4M2O;ItY-q|W)@(x!Y?v-Yx{nk*@m7-^oD84XWX z>C}dno@q5eYae{DGkP(Tzk4*qUuFf2Wbmt;t2d(3x8d5{8I9LVR4VHaxp3)2%I?=ol^C<@FkohrOl`d+kjg8S&Y3Wv#-dJlb#L@7J zlB9&Q|LQ~wl(Y4>UuSPrr&ShmlYX3D>8$%)6Ierj5%ZPipY?Y#T4d!;ygPE%_tg9P z*E7hc;TUU?L2cxBah>0hEU=dRuEb-HXW*?d+^hQkcg&7fCY1vb1%m~p)L!j8)C{v0 z^Ylr8J`!F%EP{a6>(WZZPLsTRO0d(G3QEfk$X|KbgPL1vca5l;uyuU!NPzr>&)5R; z8E5RueSLsex}F_wTW(!xH0{uZ>*X&HB9mdxxXEw(eGKZvwIZ`VCKf@912CWYbbkc5 zTir9ZOS4JxY_z==Y}q^6T}K<~X+ETf9ol|enTXWMjtY;QX@l!-lY&1O$~A1nC8&q= z%iP^HV&w)4$HyL~K&h89!_9~%CzRFyJ3@bg`JXMH-E8#RM;deq_PF*w=&Z)aw7lkf zy3${wp_`VRvtR2Iwuvefef0X*EGX;d*Yn5>BBemLA>U?>CK?|=;&RxpJhw>XDP zU&Na|7OFUEw~1MXWa#fDC)u~UjT=BeSC;PPxJ-1R^`r4y38mo8|7DDK9Sb+=Zz1sf z4THT-lpMTJG$&WTlv@z1=#%P=8}GP*3VVHn-6Y!c!o9_li%e92DUMe8_R{?MlOF7p!$^Kl{K?`9Ff`9~^F19(PAIE}pG#{H#< zJRffsq*nl{kxh921NYAewy&5+EYuY0lHyw73B}U_ny^@vh5a<;O(iGGD@K>s*?9Dy ze{i^|{qb&pYj0CYSaY2l|Iw=saWI!R%O9yv@W|)S=FK~>=zi#a?+_Eus*_b<=b=7k z_RIoLH&erA8VHE8pj<$xO12D9V+glDd3?*(#mv>0B~7Ui6*8-8t~azVdeej|FF4`Q z@~Q{fUmAmASa?tLn_zZ=Sc+uZ+30D%fX$tYy*fH&#k_KCkh%B52^S=peVrV3<@Ohw za#VfKAz3Y<0n0W`nRkgC^U!~b8k8|HS@K=(Z!vb!HWeYDKkh4%eOw2kK&!^gWxGtZ z$tqVZ)OPHQHE;QCKQ-7IZp`jdkq#QWI)l9Od)$cQ3AzwRJAT_8IkRR79xJ*mmS);_ z4l~rt2a+qetsor{6OPWk?UdCXsgbe@4HcFIwK9{RMRzR_jXkNa?p(b=$rz1XWQ(%;jkg@ghWw3O8#@CtJKX8}wi-r%w9s(Y7A z3&>ZTBeERS&SVzaXDbe0D$|eP2^Lx|YsjlR9W2+kc+fu+0y%3lv~N6v23%R88R7zR zfnZ<91C^$=xu;&)BkE&QMED!5j^?G9a3 zvs_UrF>EWkWIhn&pYhCNVE%7nK4QhceKu(%(Sc3)Urn37lbNLbiE`7T#go_LXAb{_ zioo|2lipQ(w>9J5Tt17o zTuwcW4tt{7UosTujaxI@5qF}sFk8pJSFpZsBx$VW>Jv+g#od~5ELp?8<<@;B<@QlA zCzBmo4TY7;D9LLI5o0gJ2D@_Krg@7ers&B(tAVoC5wyPeTg{uNeCg z;fv^$c+NhcrQjOB;IP{ve0&99`|F2@lalJNLJlF7arY#~C*|>P;=7QljoQ>wrfyGQ z^y|ZQkupu$nJ*`Nad^}{{if%WtR` zA!7N&<^kHc*o^w@+EEvA{|@1J%1+TLwSGAeE_YAKwC!lf7PK_zsGho~^Tbpt(6K|c<$B3$Spt^TJPfDg8&@%VX zhi*n`Q+DT^(Ty5)Yifdv;&W>42I6$06g3r@4~PpQAE( zm|VLD?NJz;3q}or{gE11eEY5a+udWmD}YlK=2PkqvOTTBudy>isz*&h_&%oDM)OOfgZy*T-k6#^jWc@__yuG?yjxab?o0_Cc3j5t+67Gf%0#!2H;p^@lv_ z!+bQH@g-DwWcz5r+3Js;;`?UT(0>+xyI82RO1XF1s^nCX)?|&xK!!7+JC>Xl(&^=* z5V$;b3!XIjjVTMf?q8er$%0Ti`qERWrcc|{MI1M?xU#!P_s9?rb#sQt2IOt2oi`|FngGHAe^k{4FT+}aWL24PUwZT?9d!8tA zE9UHS3fD8ZMvy0l4dU3OuzP`Pd&?o4zal>n-JILLP@&^&e@UWN4pVZ?FaOQ=dB$1W z+4D?i%99(MAeV~-kiT3IqdmyHE!GTOq+DIn<@GQ#Rpo?3q&SlRhfSeEf0gY?y*Cm{7{qH zY}vS7quFMrOxbF>(2%C8K__DUOHHBW+C7wO3G5H~;FjE1l1+Ivf=j)M095ss`YE^H zjb2fM%f{g+6RS!SY}R_ja&5{Ti{UTz)WFRS2Ei9y(UBkTEtd{=T)R7U`cQ52=&GFM=W=Gv384?0WMOhwWzG3Ns*rb~+A6aV{Mt0nC6WeEXq_2@vK2Q9d z3F_v`TauAEV|xtA!}O3p&T^8fps%6OQgc$}xctdEoOn^l6yV~nuZwGL@oDo5^{qhk zLGd(CnX~IjvCSSKgI)k4>T&EVpHNxJDgtm=+w?H;LHwm_pL~NXAelt-8c4}I?fI;k zN2TjSaDZ%7gCOs)uPtgAMd^8OMHVK$_PRw;Dew0 zFZ930sjp(vcJcuTk;YW`kGeglj)_1um2qH-FKLTubK9h>( zbFcc%L69HUQNR3CYlw`z=0o(?8Q0Lw{o}T#jjHA<3^}pYr5UttB~?A59E=!7(ygwv z{kFN!#Dqkl^M#&U75^Q1%#NPA;UV;Qm*9dk!o79J6!y|KMs&$11bH;&7Xh5u`^J7@ zSn(XKOFB6+NwAdE!AVE7+6V-Lm)m48kZxT1v9u3-D3RM02GhrLQt~0)# zb!x%FQUXsMpGkgT>rfhecG1}?6p6L-4+~nBc8~IX`rnbLwgA|(SoVc^pr#Poc;S3} z;4!v!cj3rMy*kA`a1q_uo;FX4$)$8cvK}VWgWCTbu}5EcmWi5ULk_B!=l);&hsVgX zl^@QB?K8ej{>UA?+i%>Bp3aa(yX1NBuCYeel@&*FD>B|xkaoDT3Rsuxr zV%U4zrBpOtYq)uk>zic%t&s&5k~%eEm-;nxI#wBor(OX`4UaqhHe$C}F9Y`(waAMj zRQEw?Y;5Fk^19>58CH!M%1b;$mv}lK?&IY7BtLF~XnozR*eUu?)sD$7affcwkNClV zfj3s%3GbG)Cc2aUP>*mfcv?|IG4LN*2s4#V+&_2zM(v;6sWphdXIaJlM7>P?pP zVMx8aw(X?8Jo(tlBmbkG>L1*1JC?=Mm{AwY@fHHOpU&Ua&Yl7}A|zG2Kd%FM5vv`$ z9mqs!a?L<@x}o)&^?aX_kUii=IA~lt7>unkC_vR}fVl28-?ISdP#HB(h0T3X>^cj*i|Ez|`d<3F4VXP<2CO-kqw_qict70)UF%LBRdO0iX;< z9ckQY)G-g{mJ?M*jxJGyMcPme=TnWTVhrwHS`JT@nUEoBc;Swdb$I*z4xvwXd}+3s zTL^dSqE=nt_FS>$*@G6E(?Lx!?C-u{o$S_#y4KEyCbmk^FF#lmyO@b(ijeMy#| zhdah9_gZ_cAX<2Yq~Pqfm`uztU2~CkxSQ5XBFg}xa(aXKCUI3Vg>^1bkJ2EAL(~p7 zer+`tg`iqxId}1BtBt{d| zT_M)8YX<*U(V4ijm9~F;-dW$NYNrbZRnsbpVwBLW#ZpVGNh}eDlGYMhJHhWq!Q@c8I>&XlE6XdLInh_*MC;0Bo@+ zOmCP4lpT?tr=q-Axr#YyApN?KLeQ2)OWjxPJKf5n+kT=Vr594_-M_IU>IQM(RN&i6 zxFQc5e=kPr4g~B8g?1iv?|#Zw{EnG*d;Kr=ksr~S!P=|SvSsU(w(BMoPy-`<>KyaG z?(WuNg-`hJl9ao?jJ}Ow`+V1rYV7MVDzuK{UWFmSPgC`2e^&J!%)V>zZ{aVB2;_r# z5^AkSFr+wNu2)nJMsIXIU-<`(h=^FTb}OK2z6SH6D+}fv#h(?os)+HzTm!*uSymO? zl#h&mTG3lOig!$+kzUCUksX5ORtms{mFBkb)6})bd z#I%WWndhK4Nw6h%v)%!H-wnR#%eF;{fo*RLYx^DB#IPb ze8a)T4zCvx+9DQ6&#_D9EfbD(U0LXZT;@I|&ez`UMR)jwSet>qBh>}w;LeNpy&Nc7t&` zJNKO7R@^;AUAk(_eRd%L_^4 zYr!C;qGpPLrN_@okq*{?Yv|2ns{D>1Dza!wreT|e-dJBZv<8J8yh1NIZvlQQ?NHhq zERSp|Q2@+DGFSP)#6QfSDbBUy_go^^IAWDf@M1`12riT?M%alFn>?VA!SjJWl1qI@ zn0OIe+LC#7mvy7FZX{Z(^vtlsID&fg2cHv=*~bkFu-*c5WwXAGF-GtMVWW;m;!-6l z?`CDfB$^fn3JQ(QZHUzpndRF-&peqNeW8SF7DIoN&c4(Rx%yOMMJ`!f(xmXLv z(mJ^G?~U!&(Yh2fbWswtKRhW!+Z;K3c8dB?LdIy!;@rt~OdLyoo%|^=czKt*luaZ?qa15`8cm)$0z5WMQ7yVt zp-rWlP_f?z@=FCz^b)2^9DQ?osUyn&esjvD2fnrOD?fA$}7U@YocTDGj5hH~N0 z`wIew%@}E0=kNrM8c%3hoe5|-T`*9StLW4Jg?qVed9)0Oz-dXG*x83ZLz)bP$t&C)g6^IA z6qyvbBG5S}g5}=W`g{F9M+J`31BzrRkno8k-+XJc=p$O*C))>$DM?M8pjenw^k~)H zkyI>5L!g0XT${A(X}64$D({k&e3hQM*f)7VbQcXeX)il(qH@)Dex_kp>(AwF<4zjN zsw;A5XVCXsqF7&@ww$B8?oE1mk#Dn$3bsnH^`sF3WAf;`mw ze2N?u*8MuoPH!+v@kQJVk9vcu#~WjU#xcr}sFQQ=haeX!jOF+p5YKje$58{kD<2(Y zCubMcvp%+D@qEtidw?RNe___xZ@EM(sb7)gG7I2$ZLC$^RoQ9=Rk$^futC8QAMXuJ zksEbHO=pC+N-mxF+QRU_D=E%tNe(w-1v)*h3u;C!{=~}`4#PJRx(jGY4!u}&3i>dt z^))-m#+OtNtM;8|0c!XUH6w=lifqzxg>w7vK_%E_HsbEUvQ zOmP};7PHPXhdY7;OHa4n%MO2%Fby0i%El$$SHX`oQp`_#IE0p|IM?`pny=9(MfB>( zsmr{RsXJ@Ewt)tdv1?l)vT|;n!7(iKwtcBbUGS>kk^uvWIW|U2t2{C`Yq8(i65l?h zm5A7#PhfuRVuT^s^Q(^yppJ8s)60BkgfApkmH4&(_1izeYW{5xzT8)6W`Fj=Z=~!l zfrZ9e?SJ?_CnpC;{UHyNe%VRw^1L{_u|P1)f3l|ajl$+n@3O}+&R@7|2$jCJ39~dt zuIa7wp!gG$Yx9OhbBC%21qrY^dyM{vly_5iA?p{eqN=&8Gnlvj8Z!jR?}k|IC9JkJ zAjCM})@CYLC)>-p=DGb<3I1=Je#~`Sy>7ox&tSM_=#_njpI3=*E|pqabLmdJCmYmK zm#d69-pli0v5wFURBAGr!W_<6ssi8piJ^(vRE-`wVx)CF%4M?Z4#_oi5>VX+9^~B; zFF{m6G2Q>$+fg^iS3Q{VBQObq`IK(a0R2BvbD7uWgTYMxBU@?5LG zJlYoOkLI=T^gLN@N353Qvjg2+)v<_ug_g-?$M5Q876M-U%v27=&jP4+P1gafs)MT)rYcIw`sj-EF0Z7z z_ppzce9x=KD>5@uY&~qk z!hk&kTy9l2qH4uzNaY`?eMkn|5vUts*O`shGg|42y!MkK8a_>>u8ZdDvTZPmOBB4l zq!zLjfl=wEkdEaJvBDJOd|A-fEQj7T2P#JlUoe%9?{bkpDR+#2Mw1t!et#!wOD7Dm zLP4ZvtDx|Z?0KF+XmR@1la6rTX0hW|Q139K<$2L-Ymi}X2_rHu+vcD=PFX!x-p-`=x7RnO5y~mU~kph z`0JqnDpTrtnMrR84s>_#s~XG>FR3!+HUr9-aZ<@$Da!zPT_1weca>Sh$C|7Tx3e2y z%9M!Z$%Y8Q^W{Wc;pYl*HsJ;9deWan;M_Tba2S|i{xUA}u1p3fUJzIf`nt#q4ax|I zZ09{X>_<8bKFcpUb5PW2jZSQ#h{wl8J9IEua}m)~_*48qjo9eQ2hpi|-kaQqzz5+yz{` z*A^Sq@iA$|n8~KdDFKngdWhU!peR5YD}<$#oZ=RonbR;>%LH!iVbvZ<=pdI&7>F^ zp<@Ko8lf`f=a7;9b_7l#0`9%U&$7v(eT$L8m7xo>@9jW|3mo`>%|3r5GVb?rKdZHM z*FcWrx`!zQ1;0pV$SvIe+`6fOtNpl5QdK2;!u?V}aU=PyXu80Ea!?FMMsD078v{;8 zU9J8VhE61h5Y1K~&X*-^ML7bB?rbhNFPtbsb%e_J193QU0G!hbEi$C5$j=j=Psf$; z*m;CgG5pL6ww#z{e#A7$bu3g;9#UDHCg4P$$Lh12rO3DJn1;Tb}?xe)pEPjWuUOMq01zU}_%poQ_O-i~lvk|K|UCw3qbSTWr4!v|Fi# zULHSSit+OjQT{&pYHaa89T#6yOz&@9`+F~O&F|*C_Kh6vs(81*lYWCfNT2R`eIJul zng~nAZxysa<9(Gry#)hW;h@L($OWyWkV8$HpQ|FE!qq@16q{Ao45tsrQM zlk)WedN21K+J@Z~aWXHt{mbe^jF7`KoV80M8=MVn0OB;P#rsS1Mpv;8OUR1xn#UfC z*vmfK>L9Ope@>$=44&DZ6w*Su*eM-)!LOFdW?PdUgqEa|j&PM<2oDx@^bett5P2?Yp$EOL|xF-983|)Vuquj z%+c!ryH@0^nyNtkF;|%l_N;|tQE8|-dAi_(NYbx+C3<85YBbg;rCrNciO=*2@=M_F zYMDPEC4ES|xF#P;F%2vWPxaxg;3`X?lh**%yvO*Op9z zS>tdjay^=s6g@V7x-W&b+vILx&H;-iRsw8q3b8eGcW9H@sh?-e8+Em^T=rF_4hMEg zDqXEsh|%H=1TJ-}TSvr-=hjuWzl^KJ ztO&Sn2Gyn~AeLv>W~chCFR#pADLklHhv<{{ZJI^|x&7bW1JuuX$qGs-R!4VMy8^sk z!3$5o+IU;UioXsw<3fot<;8WI?5J6}@X|x5LEo$T%J*}xQn{9R4IU|*(`caz{u)zH zGmg!BmT?%{)v23VLX4+a=c6#7Ic~+xUKN?#d+WyIDVIWhu`o`i`{0Ub`$FjH(WMU!voxakT$3EhwkYXJ^5AI(Fx)&uq~KM6?yUS(#f0kht*vb_&1c# zmS+E~K8h~RM$X7?=G*UesxCFMk`-$Lt1us^!B*gH-SLH(EsN{l-!t3}`V~5yQZ^CC zANqujf_8TM!1T`Z@lKIU3F6^Zs|fxFQML?J{)+8;(jaEG)cp%}RD#H5Li1|emC0Tm zMoA$OfOCFrEb|8$T{;ai!}V_0%Co6mFHIfyy;b-482}j2ZDz@!()TH4+5pKLwu(n2TwcCuv#VL+k2TK_eT)JbMl0 z-+z`d^0K|l=-w#+g^v$|i&%!x%qBoIPteo8Gl}IL6)0IQOn=dZ`oukI@UAU2(K{<4 zNT*Hci+(|Q{9surp&UcKYPpR=(nRw?E;p%|2ks+x z&#Y7%$H?f@my2FAg662fwlOPR=5bypjh43I16q>Wvo|*DxI&syT?bJsbJ`GUk6Gcn zoU1#KFR!b!yep%By2GRx#}Aw27mMpmX|!Yvk!n;MdblZ1$O4P^j>&P9IatsETts>x za?77A=TL9-PZS@x+|UGMsi~)dQ6QFF$3WNt)*$=!Fc7hY1#;K%Pwb?rwx3uL`}1b^ zGyMuk$0QNM>F$uzxmV1Sb$NsuExowIu1`S^8>a*ZddSar$YZ>aU9!$O>Q4;pCj~jX zL|v13MTS_}41C|u=BuL$`^v=P`>V82+00?#y}ID!mq0a95;jj0eB30{IAnkJPX;Xe z|7gE|%1OvrI4?R_L6kqR_hf{wZGPK3#g+dk?`~qPAH4igGc+CIy71!kglqGW*{i|% zKuRDO&L9SE7R==1*~met%0uVdg1#@$@J~l31hAe@rTBNY_f?bD#?m7nyu!xET3?gG zT+(eL+S|UI^O6jrDkpC_a~6CKZX7ot5D$V#{o3i&EgDWQ(HLCu6G@Svpkxppf)B)1AU}Uk|r&>U`fKh_2 zMEr=TJG`t4+*`fN{-OJi2Yb{%niW6QJuGw5m0(V2tvn%F+%ZpVKQcy+dXRf)`zDcc z$cu*0{Aw1d+w;Ix9CNrDg^y!vBv&a!r)>$YopD}NDmD~F z_#g1c5f7_xf1u`T4k`CUatE$vJ2!`l<_XQH8BsIwnF!d?j`&FDjuh)3)HZRS#x~`R z>>K2;Ru2(ltU=W1r?swmm=1DLH-xo8JADXlAN85^^Diw!lWF88x*~AFo zlNsgZHn?a~feO80L}*PkA&l6R+dSoeZqs*YSRoCo7lL#mmrXa=zvdLTcu|g%YR*fc z4?LHk2s!swUudz?e3kcZEJ-+x1u>r2-SDvM5*BYoxxpg)bT%8cVLrLaSu2dPum_1q z%R8V;y58iFQ$C+**a`npUN^1HjTi|jfj1>A<@vW}iE?6TPEQ*~el{nn;&2r)f(}a! zeb==C=sV$Otd3u1u-INtBq4!~Z`i?QB=LE4r!l%3keZW+=B1Kn&FnOj8*Vl9uz8b6 z9_uu}m~OIg7X(9TU*ZEJ>h0>f5JFmfCocG^IL|eJPir&Pv6AXVF2XC$W1g?uErNr1 zJVRADs!3xG)VgXS7LnjqKpb4;$_dwxsx@(e2_wY$2o)rA;-qsBox7KVGH-;dP7&`* z5`dTG4an#c&)ZeFKUKs!nd@Fu2H6xE6P~dUso$orp>O$YkmA|5SKu9@wvep`4H5Tx zq0Y`%-Bko%rqN+?1Djjmfx``?UtmVkFQ4}WOxeYh;`p|Exz=p__?>{9`GV>eb0FTD zbdeilymcQlWfdxHJ+YJRoIuTCKhl?t9ec0`(Zg0uk2U>FmE6M1*$Fm3-nh*RJHwNG zc+bQoyf--@<4+prU!C|G_CXkzQdY(8##r*257juy(Q%j#RMM1MuA{g zu1P$+aJ!&jrA=yA-AqN!Wc^iVe=*N3OB=t7{OJRb#!`Hw^yKT0x?dr;)|~^N{9-!I zgolY==ItYcMrXrPB@-7o1*5nx-Kri_3I918Omy*_lb&f1>W{wR2B>dB6o=OV>z4ej zdU^No=o@Na^^=3k%L%^K{`js|3K}56Coz{-&wj)dUIHN0-+2bOtH1G2Ec%c;gezqa zxCZ`}WSlK|_#ZvP9*c}R$kDZn&bAs4Brmwu_3c|r@uc8?UW%c&7gIofHM~Ic$sM<@ zRyE?Oj#@kNl?mAbD#7KWx#GXy1V)p&2_M!2P_Amkwo91>a5-NNK2T$Ul6V_EcZ74c zOx4h7x5PSa%CJec{i8RKv1YZY-`06tsVUS!A%~5xU@p!U$_HJjubDzOIq6B00wsyo zM>Edr94b;s$kQ3cPkOp`Y!hguDlHIIRuNpO#|#yI#;f_^(}iD%Nj}F8Yd}OZzZzJa9REU-DYid-*DiB(Nr))yq!+E;})15p!qqN z@howM69vTmQFb1$22I_u+iv^#?>FD?U7HV1CeH1~qsgBBcGTgPZ>=+@G+sx@Ime)< zIZ^4SW>YiJ#IW)6{?osQ(D8>_rF*Q$xu0+F!Y!xg55qX7WvZxOtF2y^JZ!OYJNsZh z=j;{Do5An(^Som%d_SsI&(40l6q|_}3#D0re5p{`heLijGmK5_LO;gqW=K7Z@z~aF zUdMybX_du3ncXX1iD~CMdfnma{Xa&PgB9Z*u2?S__~*oohFGZx`@=uQR-&S5aoo<@ zb201apft{Iu72?3gAvDM*nj{vg5j;TA`L`akgtjFLqB;pu_D&lb_rlHYdQP=aUd+^ z3Jw5vTr~D?nAtnAQ$xz=6D2L<>)*5!XYKwvHjKGYdf2QP;+Ft0s(*jv)?7ODyq316 zK7PSrr?9rq5gu*N!UFGp-n%;#vqr$2D#D$(#dggXBc~D{G)y@G4Fhpm2IlB6-EU|O5 zqXSsYa4+eS8(7pWW1L0-JpGD3ER+Keibxw-4YSsF0&q*-ZolvGa}FjYWUISt(~+~x z6}SvI0FHz-^KMZ&dC-$9^qa(1Xk5@szb3ErY1|H!4)YuETJCD{6fo;SCmk%qLnp|KIEFK{%M-+vvSXp;nBFV*eZl}R|o+7p~l z5@_E2CBjw({_#3^>&7_giHI}UhcL}U@OwrFNAIY$`mHFOGTJ2peam38J^}CIx;eF1 z@~Sh4cf(W|0HwCw(|jdRIp zHXT=SsQVt3X)>N6K_i0k3TNW>`N9R?4FTbVLmTK1-m0fD8bLcimh{{j-W#(+1cUVU z1#*cKZZ>@SE1C$xTudWIN?!18iGAtA&E@pRqIo}p-R7Y#WS6XsVh~s!UBfHlsR}W( zn4NW3kW=!AmB2Sn0GhEleR*x5@p)k=@zUdBmOfC5;#R$AqSw3Nid>hL;liA_r=~bB zj|A=_IQJ#Ng~AK5q=n}*nq8Mx0KV$8bD8K#|08et2CZKeatoNJj111H5wgRLJKh7Ove)twVQP>@|VYioYPa6`I%}k z>@HcKZiZs1q(OScE^7l+7`9rfwq%8h2(OS_pWho%IUDCaz;&l5E=1LGj^~1#y~~bH ziz~wq1M)F&gq=U1zoGFA_)HbOl#H`0&sGgGHf?>+6Kzb_o#zDADS5pWMF0qj&;aSK zTEpo>*S1F5Qb>*gb8GO(`ogt!`$DkkNP$`}AwhgQQX!#{KXaT4^;tL!kkr(+6#1X4 z@<~F_8wj|9n05EWh>noRAF`?(7QM0~`3>(>snCr0-{#LdYn^JthdO-DmC#hyM2U`Q#1I+Fqcc};bD!8Jrn(5nAO2{w0Ga2O& zzJ6>GBd(T;g^i1X)E?=*oD5T(hj9WI&Yv!zP4?bdH}?sX@jbUP|EK)8{R~1X;iS=5 zct<(S)scMZwBb}4DE{*7GOd%<(>7IIPpVk~c#K7yC_KX(Nh!mg7?tqM;ApC=*qJ4% zE3ONKO=c`PvT$*QXbBJ0x_w&MzIMXwYbb$?!`7Jbh6$N#e6eBt%WWkTh{O}-r9iWg zoC=4AjNw#&fnjU-p^9*CYBnS$t77dCjSz}k@+(E0JfErAEY2^n=0q*R;0i^z4P;qC zifn2OIo;ZSZIJnKCppmP5E^&^U0G%Duxa_Wj}6nsk9nI*#8KH~ak_f^~3@ER6eW_d19ZhTZoiW%`;3 z`o#aOi9ZG1!Q2AkBePN0dZshmIDgst`I~|wcJJe-8Rb{AJ+~1XC=S7?l9Srxf5L4U znaw#nz5RpTmJa9qmA2IhA<;lecRc$)K~gCre>2oTk088wK|97(>0l^5_DMHA7wB+& z3osIv+`QE&0ZC!eXPDy(;OUY1nq7cc3vOxMK19M8k;8HIm2@6k~Xh- z>2ept+tQg(N@Z{mmgiIo`lB&na%4LWOm_SXF(}joOL7`{gzUbWs42?LWtV66ltC8g1D}mmC%xIhZ3k7iTNWe4-P_T9QN<{{{v~AM>zlh diff --git a/src/ADCUtils.cpp b/src/ADCUtils.cpp index fde8b14..4d19337 100644 --- a/src/ADCUtils.cpp +++ b/src/ADCUtils.cpp @@ -22,8 +22,8 @@ * along with this program. If not, see . */ -#if defined(__AVR__) && (! defined(__AVR_ATmega4809__)) #include "ADCUtils.h" +#if defined(__AVR__) && defined(ADATE) // Union to speed up the combination of low and high bytes to a word // it is not optimal since the compiler still generates 2 unnecessary moves @@ -35,7 +35,7 @@ union Myword { } byte; uint16_t UWord; int16_t Word; - uint8_t * BytePointer; + uint8_t *BytePointer; }; /* @@ -45,18 +45,18 @@ uint16_t readADCChannel(uint8_t aChannelNumber) { Myword tUValue; ADMUX = aChannelNumber | (DEFAULT << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode - is default -// ADSC-StartConversion ADIF-Reset Interrupt Flag - NOT free running mode + // ADCSRB = 0; // Only active if ADATE is set to 1. + // ADSC-StartConversion ADIF-Reset Interrupt Flag - NOT free running mode ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADIF) | ADC_PRESCALE); -// wait for single conversion to finish + // wait for single conversion to finish loop_until_bit_is_clear(ADCSRA, ADSC); -// Get value + // Get value tUValue.byte.LowByte = ADCL; tUValue.byte.HighByte = ADCH; return tUValue.UWord; -// return ADCL | (ADCH <<8); // needs 4 bytes more + // return ADCL | (ADCH <<8); // needs 4 bytes more } /* @@ -66,14 +66,14 @@ uint16_t readADCChannelWithReference(uint8_t aChannelNumber, uint8_t aReference) Myword tUValue; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default + // ADCSRB = 0; // Only active if ADATE is set to 1. // ADSC-StartConversion ADIF-Reset Interrupt Flag - NOT free running mode ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADIF) | ADC_PRESCALE); -// wait for single conversion to finish + // wait for single conversion to finish loop_until_bit_is_clear(ADCSRA, ADSC); -// Get value + // Get value tUValue.byte.LowByte = ADCL; tUValue.byte.HighByte = ADCH; return tUValue.UWord; @@ -81,31 +81,36 @@ uint16_t readADCChannelWithReference(uint8_t aChannelNumber, uint8_t aReference) /* * @return original ADMUX register content for optional later restoring values + * All experimental values are acquired by using the ADCSwitchingTest example from this library */ uint8_t checkAndWaitForReferenceAndChannelToSwitch(uint8_t aChannelNumber, uint8_t aReference) { uint8_t tOldADMUX = ADMUX; /* - * Must wait >= 7 us if reference has to be switched from 1.1 volt to VCC (seen on oscilloscope) - * Must wait >= 6000 us for Nano board >= 6200 for Uno board if reference has to be switched from VCC/DEFAULT to 1.1 volt/INTERNAL - * Must wait >= 1100 us if channel has to be switched to 1.1 volt internal channel from channel with read 5 volt input + * Must wait >= 7 us if reference has to be switched from 1.1 volt/INTERNAL to VCC/DEFAULT (seen on oscilloscope) + * This is done after the 2 ADC clock cycles required for Sample & Hold :-) + * + * Must wait >= 7600 us for Nano board >= 6200 for Uno board if reference has to be switched from VCC/DEFAULT to 1.1 volt/INTERNAL + * Must wait >= 200 us if channel has to be switched to 1.1 volt internal channel if S&H was at 5 Volt */ uint8_t tNewReference = (aReference << SHIFT_VALUE_FOR_REFERENCE); ADMUX = aChannelNumber | tNewReference; - if ((tOldADMUX & MASK_FOR_ADC_REFERENCE) != tNewReference) { - if (aReference == INTERNAL) { + if ((tOldADMUX & MASK_FOR_ADC_REFERENCE) != tNewReference && aReference == INTERNAL) { + /* + * Switch reference from DEFAULT to INTERNAL + */ + delayMicroseconds(8000); // experimental value is >= 7600 us for Nano board and 6200 for UNO board + } else if ((tOldADMUX & 0x0F) != aChannelNumber) { + if (aChannelNumber == ADC_1_1_VOLT_CHANNEL_MUX) { /* - * Switch reference from DEFAULT to INTERNAL + * Internal 1.1 Volt channel requires <= 200 us for Nano board */ - delayMicroseconds(6500); // experimental value is >= 5000 us for Nano board and 6200 for UNO board + delayMicroseconds(200); } else { - // Switch reference from INTERNAL to DEFAULT - delayMicroseconds(10); + /* + * 100 kOhm requires < 100 us, 1 MOhm requires 120 us S&H switching time + */ + delayMicroseconds(120); // experimental value is <= 1100 us for Nano board } - } else if (aChannelNumber == ADC_1_1_VOLT_CHANNEL_MUX && (tOldADMUX & 0x0F) != aChannelNumber) { - /* - * Switch to (high impedance) 1.1 volt channel - */ - delayMicroseconds(1200); // experimental value is >= 1100 us for Nano board } return tOldADMUX; } @@ -121,8 +126,8 @@ uint16_t readADCChannelWithReferenceOversample(uint8_t aChannelNumber, uint8_t a uint16_t tSumValue = 0; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default -// ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE); for (uint8_t i = 0; i < _BV(aOversampleExponent); i++) { @@ -132,7 +137,34 @@ uint16_t readADCChannelWithReferenceOversample(uint8_t aChannelNumber, uint8_t a */ loop_until_bit_is_set(ADCSRA, ADIF); - ADCSRA |= _BV(ADIF); // clear bit to recognize next conversion has finished + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished + // Add value + tSumValue += ADCL | (ADCH << 8); // using myWord does not save space here + // tSumValue += (ADCH << 8) | ADCL; // this does NOT work! + } + ADCSRA &= ~_BV(ADATE); // Disable auto-triggering (free running mode) + return (tSumValue >> aOversampleExponent); +} + +/* + * Use ADC_PRESCALE32 which gives 26 us conversion time and good linearity for 16 MHz Arduino + */ +uint16_t readADCChannelWithReferenceOversampleFast(uint8_t aChannelNumber, uint8_t aReference, uint8_t aOversampleExponent) { + uint16_t tSumValue = 0; + ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); + + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE32); + + for (uint8_t i = 0; i < _BV(aOversampleExponent); i++) { + /* + * wait for free running conversion to finish. + * Do not wait for ADSC here, since ADSC is only low for 1 ADC Clock cycle on free running conversion. + */ + loop_until_bit_is_set(ADCSRA, ADIF); + + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished // Add value tSumValue += ADCL | (ADCH << 8); // using myWord does not save space here // tSumValue += (ADCH << 8) | ADCL; // this does NOT work! @@ -149,8 +181,8 @@ uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t uint16_t tSumValue = 0; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default -// ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE); for (uint8_t i = 0; i < aNumberOfSamples; i++) { @@ -160,7 +192,7 @@ uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t */ loop_until_bit_is_set(ADCSRA, ADIF); - ADCSRA |= _BV(ADIF); // clear bit to recognize next conversion has finished + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished // Add value tSumValue += ADCL | (ADCH << 8); // using myWord does not save space here // tSumValue += (ADCH << 8) | ADCL; // this does NOT work! @@ -170,16 +202,17 @@ uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t } /* - * use ADC_PRESCALE16 which gives 13 us conversion time and good linearity + * use ADC_PRESCALE32 which gives 26 us conversion time and good linearity + * @return the maximum of aNumberOfSamples measurements. */ uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReference, uint16_t aNumberOfSamples) { uint16_t tADCValue = 0; uint16_t tMaximum = 0; ADMUX = aChannelNumber | (aReference << SHIFT_VALUE_FOR_REFERENCE); -// ADCSRB = 0; // free running mode if ADATE is 1 - is default -// ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag - ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE16); + ADCSRB = 0; // Free running mode. Only active if ADATE is set to 1. + // ADSC-StartConversion ADATE-AutoTriggerEnable ADIF-Reset Interrupt Flag + ADCSRA = (_BV(ADEN) | _BV(ADSC) | _BV(ADATE) | _BV(ADIF) | ADC_PRESCALE32); for (uint16_t i = 0; i < aNumberOfSamples; i++) { /* @@ -188,7 +221,7 @@ uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReferen */ loop_until_bit_is_set(ADCSRA, ADIF); - ADCSRA |= _BV(ADIF); // clear bit to recognize next conversion has finished + ADCSRA |= _BV(ADIF); // clear bit to enable recognizing next conversion has finished // check value tADCValue = ADCL | (ADCH << 8); if (tADCValue > tMaximum) { @@ -200,10 +233,10 @@ uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReferen } /* - * use ADC_PRESCALE16 which gives 13 us conversion time and good linearity + * use ADC_PRESCALE32 which gives 26 us conversion time and good linearity */ uint16_t readADCChannelWithReferenceMaxMicros(uint8_t aChannelNumber, uint8_t aReference, uint16_t aMicrosecondsToAquire) { - uint16_t tNumberOfSamples = aMicrosecondsToAquire / 13; + uint16_t tNumberOfSamples = aMicrosecondsToAquire / 26; return readADCChannelWithReferenceMax(aChannelNumber, aReference, tNumberOfSamples); } @@ -219,7 +252,7 @@ uint16_t readUntil4ConsecutiveValuesAreEqual(uint8_t aChannelNumber, uint8_t aDe tValues[0] = readADCChannel(aChannelNumber); for (int i = 1; i < 4; ++i) { - delay(aDelay); // only 3 delays! + delay(aDelay); // Only 3 delays! tValues[i] = readADCChannel(aChannelNumber); } @@ -261,30 +294,30 @@ uint16_t readUntil4ConsecutiveValuesAreEqual(uint8_t aChannelNumber, uint8_t aDe } /* - * Versions without handling of switched reference and channel. - * Use only if reference (DEFAULT, INTERNAL) is known to be at the right value (DEFAULT for VCC and INTERNAL for temperature) - * and register ADMUX may be overwritten. - * Use it for example if you only call getVCCVoltageSimple() or getTemperatureSimple() in your program. - * Calling both will lead to wrong values since of reference and channel switching. + * !!! Function without handling of switched reference and channel.!!! + * Use it ONLY if you only call getVCCVoltageSimple() or getVCCVoltageMillivoltSimple() in your program. + * !!! Resolution is only 20 millivolt !!! */ float getVCCVoltageSimple(void) { -// use AVCC with external capacitor at AREF pin as reference + // use AVCC with (optional) external capacitor at AREF pin as reference float tVCC = readADCChannelWithReferenceMultiSamples(ADC_1_1_VOLT_CHANNEL_MUX, DEFAULT, 4); return ((1023 * 1.1 * 4) / tVCC); } /* - * Will at most times be sufficient since switching reference to default is quite fast. + * !!! Function without handling of switched reference and channel.!!! + * Use it ONLY if you only call getVCCVoltageSimple() or getVCCVoltageMillivoltSimple() in your program. + * !!! Resolution is only 20 millivolt !!! */ uint16_t getVCCVoltageMillivoltSimple(void) { -// use AVCC with external capacitor at AREF pin as reference + // use AVCC with external capacitor at AREF pin as reference uint16_t tVCC = readADCChannelWithReferenceMultiSamples(ADC_1_1_VOLT_CHANNEL_MUX, DEFAULT, 4); return ((1023L * 1100 * 4) / tVCC); } /* - * Do not check for changing reference or channel. - * Will give wrong result if used at any time after analogRead(); + * !!! Function without handling of switched reference and channel.!!! + * Use it ONLY if you only use INTERNAL reference (call getTemperatureSimple()) in your program. */ float getTemperatureSimple(void) { #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) @@ -302,7 +335,8 @@ float getVCCVoltage(void) { /* * Read value of 1.1 volt internal channel using VCC as reference. - * Waits for reference and channel switching. + * Handles reference and channel switching by introducing the appropriate delays. + * !!! Resolution is only 20 millivolt !!! */ uint16_t getVCCVoltageMillivolt(void) { uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(ADC_1_1_VOLT_CHANNEL_MUX, DEFAULT); @@ -314,24 +348,28 @@ uint16_t getVCCVoltageMillivolt(void) { return ((1023L * 1100) / tVCC); } -void printVCCVoltageMillivolt(Print* aSerial) { +void printVCCVoltageMillivolt(Print *aSerial) { aSerial->print(F("VCC=")); aSerial->print(getVCCVoltageMillivolt()); aSerial->println(" mV"); } /* - * Version which restore the ADC Channel and handle reference switching. + * Handles reference and channel switching by introducing the appropriate delays. */ float getTemperature(void) { #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) return 0.0; #else -// use internal 1.1 volt as reference + // use internal 1.1 volt as reference uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(ADC_TEMPERATURE_CHANNEL_MUX, INTERNAL); float tTemp = (readADCChannelWithReferenceOversample(ADC_TEMPERATURE_CHANNEL_MUX, INTERNAL, 2) - 317); ADMUX = tOldADMUX; return (tTemp / 1.22); #endif } +#elif defined(ARDUINO_ARCH_APOLLO3) + void ADCUtilsDummyToAvoidBFDAssertions(){ + ; + } #endif // defined(__AVR__) diff --git a/src/ADCUtils.h b/src/ADCUtils.h index 1e51918..c0fe1b2 100644 --- a/src/ADCUtils.h +++ b/src/ADCUtils.h @@ -1,7 +1,7 @@ /* * ADCUtils.h * - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Copyright (C) 2016-2021 Armin Joachimsmeyer * Email: armin.joachimsmeyer@gmail.com * * This file is part of Arduino-Utils https://github.com/ArminJo/Arduino-Utils. @@ -26,14 +26,15 @@ #if defined(__AVR__) && (! defined(__AVR_ATmega4809__)) #include +#if defined(ADATE) // PRESCALE4 => 13 * 4 = 52 microseconds per ADC conversion at 1 MHz Clock => 19,2 kHz #define ADC_PRESCALE2 1 // 26 microseconds per ADC conversion at 1 MHz #define ADC_PRESCALE4 2 // 52 microseconds per ADC conversion at 1 MHz // PRESCALE8 => 13 * 8 = 104 microseconds per ADC sample at 1 MHz Clock => 9,6 kHz #define ADC_PRESCALE8 3 // 104 microseconds per ADC conversion at 1 MHz -#define ADC_PRESCALE16 4 // 208 microseconds per ADC conversion at 1 MHz -#define ADC_PRESCALE32 5 // 416 microseconds per ADC conversion at 1 MHz +#define ADC_PRESCALE16 4 // 13/208 microseconds per ADC conversion at 16/1 MHz - degradations in linearity at 16 MHz +#define ADC_PRESCALE32 5 // 26/416 microseconds per ADC conversion at 16/1 MHz - very good linearity at 16 MHz #define ADC_PRESCALE64 6 // 52 microseconds per ADC conversion at 16 MHz #define ADC_PRESCALE128 7 // 104 microseconds per ADC conversion at 16 MHz --- Arduino default @@ -102,12 +103,16 @@ #define ADC_1_1_VOLT_CHANNEL_MUX 0x1E #define ADC_GND_CHANNEL_MUX 0x1F #define INTERNAL INTERNAL1V1 + +#else +#error "No temperature channel definitions specified for this AVR CPU" #endif uint16_t readADCChannel(uint8_t aChannelNumber); uint16_t readADCChannelWithReference(uint8_t aChannelNumber, uint8_t aReference); uint16_t readADCChannelWithOversample(uint8_t aChannelNumber, uint8_t aOversampleExponent); uint16_t readADCChannelWithReferenceOversample(uint8_t aChannelNumber, uint8_t aReference, uint8_t aOversampleExponent); +uint16_t readADCChannelWithReferenceOversampleFast(uint8_t aChannelNumber, uint8_t aReference, uint8_t aOversampleExponent); uint16_t readADCChannelWithReferenceMultiSamples(uint8_t aChannelNumber, uint8_t aReference, uint8_t aNumberOfSamples); uint16_t readADCChannelWithReferenceMax(uint8_t aChannelNumber, uint8_t aReference, uint16_t aNumberOfSamples); uint16_t readADCChannelWithReferenceMaxMicros(uint8_t aChannelNumber, uint8_t aReference, uint16_t aMicrosecondsToAquire); @@ -124,6 +129,7 @@ uint16_t getVCCVoltageMillivolt(void); void printVCCVoltageMillivolt(Print* aSerial); float getTemperature(void); +#endif // defined(ADATE) #endif // defined(__AVR__) #endif /* SRC_ADCUTILS_H_ */ diff --git a/src/AutonomousDrive.cpp b/src/AutonomousDrive.cpp index 55bb579..f9894cf 100644 --- a/src/AutonomousDrive.cpp +++ b/src/AutonomousDrive.cpp @@ -26,9 +26,10 @@ #include "RobotCar.h" #include "RobotCarGui.h" +#include "Distance.h" #include "HCSR04.h" -#include "Distance.h" +#include "pitches.h" uint8_t sDriveMode = MODE_MANUAL_DRIVE; // one of MODE_MANUAL_DRIVE, MODE_AUTONOMOUS_DRIVE_BUILTIN, MODE_AUTONOMOUS_DRIVE_USER or MODE_FOLLOWER @@ -97,7 +98,7 @@ void startStopAutomomousDrive(bool aDoStart, uint8_t aDriveMode) { #endif DistanceServoWriteAndDelay(90); sDriveMode = MODE_MANUAL_DRIVE; - RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + RobotCarMotorControl.stop(MOTOR_RELEASE); } // manage on off buttons @@ -141,11 +142,10 @@ void driveAutonomousOneStep() { */ if (sNextDegreesToTurn == GO_BACK_AND_SCAN_AGAIN) { // go backwards and do a new scan - RobotCarMotorControl.goDistanceCentimeter(10, DIRECTION_BACKWARD, &loopGUI); + RobotCarMotorControl.goDistanceMillimeter(100, DIRECTION_BACKWARD, &loopGUI); } else { // rotate and go - RobotCarMotorControl.rotateCar(sNextDegreesToTurn, sTurnMode, true, &loopGUI); -// RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_FORWARD, true, &loopGUI); + RobotCarMotorControl.rotate(sNextDegreesToTurn, sTurnMode, &loopGUI); // wait to really stop after turning delay(100); sLastDegreesTurned = sNextDegreesToTurn; @@ -157,13 +157,13 @@ void driveAutonomousOneStep() { */ if (sStepMode == MODE_SINGLE_STEP) { // Go fixed distance - RobotCarMotorControl.goDistanceCentimeter(sCentimeterPerScan, DIRECTION_FORWARD, &loopGUI); + RobotCarMotorControl.goDistanceMillimeter(sCentimeterPerScan*10, DIRECTION_FORWARD, &loopGUI); } else /* * Continuous mode, start car or let it run */ if (tCarIsStopped) { - RobotCarMotorControl.startRampUpAndWaitForDriveSpeed(DIRECTION_FORWARD, &loopGUI); + RobotCarMotorControl.startRampUpAndWaitForDriveSpeedPWM(DIRECTION_FORWARD, &loopGUI); } } @@ -204,7 +204,7 @@ void driveAutonomousOneStep() { char tStringBuffer[6]; sprintf_P(tStringBuffer, PSTR("%2d%s"), sCentimeterPerScan, "cm"); BlueDisplay1.drawText(0, BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND, tStringBuffer, TEXT_SIZE_11, - COLOR_BLACK, COLOR_WHITE); + COLOR16_BLACK, COLOR16_WHITE); } } @@ -215,7 +215,7 @@ void driveAutonomousOneStep() { /* * Stop if rotation requested or single step */ - RobotCarMotorControl.stopCarAndWaitForIt(); + RobotCarMotorControl.stopAndWaitForIt(); #ifdef USE_ENCODER_MOTOR_CONTROL #ifdef ENABLE_PATH_INFO_PAGE @@ -303,11 +303,11 @@ int doBuiltInCollisionDetection() { ***************************************************/ void checkSpeedAndGo(unsigned int aSpeed, uint8_t aRequestedDirection) { - if (aSpeed > RobotCarMotorControl.rightCarMotor.DriveSpeed * 2) { - aSpeed = RobotCarMotorControl.rightCarMotor.DriveSpeed * 2; + if (aSpeed > RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * 2) { + aSpeed = RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * 2; } - if (aSpeed > MAX_SPEED) { - aSpeed = MAX_SPEED; + if (aSpeed > MAX_SPEED_PWM) { + aSpeed = MAX_SPEED_PWM; } RobotCarMotorControl.startRampUpAndWait(aSpeed, aRequestedDirection, &loopGUI); @@ -343,8 +343,7 @@ void driveFollowerModeOneStep() { /* * we had a pending turn */ - RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_FORWARD, true); - //RobotCarMotorControl.rotateCar(sNextDegreesToTurn, TURN_IN_PLACE, true); + RobotCarMotorControl.rotate(sNextDegreesToTurn, TURN_FORWARD); sNextDegreesToTurn = 0; } else { @@ -357,7 +356,7 @@ void driveFollowerModeOneStep() { if (tCentimeter > FOLLOWER_DISTANCE_TARGET_SCAN_CENTIMETER) { // trigger scanning in the next loop // Stop car, clear display area and show distance - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); clearPrintedForwardDistancesInfos(); // show current distance (as US distance), which triggers the scan showUSDistance(tCentimeter, true); @@ -376,20 +375,20 @@ void driveFollowerModeOneStep() { // if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_FORWARD) { // Serial.println(F("Go forward")); // } - tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeed + (tCentimeter - FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER) * 2; + tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeedPWM + (tCentimeter - FOLLOWER_DISTANCE_MAXIMUM_CENTIMETER) * 2; checkSpeedAndGo(tSpeed, DIRECTION_FORWARD); } else if (tCentimeter < FOLLOWER_DISTANCE_MINIMUM_CENTIMETER) { // if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != DIRECTION_BACKWARD) { // Serial.println(F("Go backward")); // } - tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeed + (FOLLOWER_DISTANCE_MINIMUM_CENTIMETER - tCentimeter) * 4; + tSpeed = RobotCarMotorControl.rightCarMotor.StartSpeedPWM + (FOLLOWER_DISTANCE_MINIMUM_CENTIMETER - tCentimeter) * 4; checkSpeedAndGo(tSpeed, DIRECTION_BACKWARD); } else { if (RobotCarMotorControl.getCarDirectionOrBrakeMode() != MOTOR_RELEASE) { // Serial.println(F("Stop")); - RobotCarMotorControl.stopMotors(MOTOR_RELEASE); + RobotCarMotorControl.stop(MOTOR_RELEASE); } } } @@ -401,11 +400,11 @@ unsigned int __attribute__((weak)) getDistanceAndPlayTone() { /* * Get distance; timeout is 1 meter */ - unsigned int tCentimeter = getDistanceAsCentiMeter(true, DISTANCE_TIMEOUT_CM_FOLLOWER); + unsigned int tCentimeter = getDistanceAsCentiMeter(true, DISTANCE_TIMEOUT_CM_FOLLOWER, true); /* * play tone */ - int tFrequency = map(tCentimeter, 0, 100, 100, 2000); - tone(PIN_BUZZER, tFrequency); + uint8_t tIndex = map(tCentimeter, 0, 100, 0, ARRAY_SIZE_NOTE_C5_TO_C7_PENTATONIC - 1); + tone(PIN_BUZZER, NoteC5ToC7Pentatonic[tIndex]); return tCentimeter; } diff --git a/src/AutonomousDrivePage.cpp b/src/AutonomousDrivePage.cpp index 0e61dd4..3dbb3d7 100644 --- a/src/AutonomousDrivePage.cpp +++ b/src/AutonomousDrivePage.cpp @@ -68,7 +68,7 @@ void setStepModeButtonCaption(); */ void setStepMode(uint8_t aStepMode) { if (aStepMode == MODE_SINGLE_STEP) { - RobotCarMotorControl.stopCarAndWaitForIt(); + RobotCarMotorControl.stopAndWaitForIt(); } else if (aStepMode > MODE_SINGLE_STEP) { aStepMode = MODE_CONTINUOUS; } @@ -161,45 +161,46 @@ void handleAutomomousDriveRadioButtons() { } void initAutonomousDrivePage(void) { - TouchButtonStepMode.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6 + 1, COLOR_BLUE, + TouchButtonStepMode.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6 + 1, COLOR16_BLUE, reinterpret_cast(sStepModeButtonStringContinuousStepToTurn), TEXT_SIZE_9, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doNextStepMode); - TouchButtonSingleScan.init(0, BUTTON_HEIGHT_6_LINE_2, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_BLUE, F("Scan"), + TouchButtonSingleScan.init(0, BUTTON_HEIGHT_6_LINE_2, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_BLUE, F("Scan"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doSingleScan); - TouchButtonStep.init(0, BUTTON_HEIGHT_6_LINE_3, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_BLUE, F("Step"), + TouchButtonStep.init(0, BUTTON_HEIGHT_6_LINE_3, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_BLUE, F("Step"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doStep); // use sDriveMode for reconnect during demo mode - TouchButtonFollower.init(0, BUTTON_HEIGHT_6_LINE_4, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_RED, F("Follow"), - TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_FOLLOWER), - &doStartStopFollowerMode); + TouchButtonStartStopUserAutonomousDrive.init(0, BUTTON_HEIGHT_6_LINE_4, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_RED, F("Start\nUser"), + TEXT_SIZE_13, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_AUTONOMOUS_DRIVE_USER), + &doStartStopTestUser); + TouchButtonStartStopUserAutonomousDrive.setCaptionForValueTrue(F("Stop\nUser")); TouchButtonScanSpeed.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_BLACK, F("Scan slow"), TEXT_SIZE_16, + BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_BLACK, F("Scan slow"), TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doChangeScanSpeed); TouchButtonScanSpeed.setCaptionForValueTrue("Scan fast"); #if defined(CAR_HAS_IR_DISTANCE_SENSOR) || defined(CAR_HAS_TOF_DISTANCE_SENSOR) TouchButtonScanMode.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_RED, reinterpret_cast(sScanModeButtonStringMinMax), + BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_RED, reinterpret_cast(sScanModeButtonStringMinMax), TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH, SCAN_MODE_MINIMUM, &doScanMode); #endif // use sDriveMode for reconnect during demo mode - TouchButtonStartStopBuiltInAutonomousDrive.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, + TouchButtonStartStopBuiltInAutonomousDrive.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, F("Start\nBuiltin"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_AUTONOMOUS_DRIVE_BUILTIN), &doStartStopAutomomousDrive); TouchButtonStartStopBuiltInAutonomousDrive.setCaptionForValueTrue(F("Stop")); - TouchButtonStartStopUserAutonomousDrive.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, - COLOR_RED, F("Start\nUser"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, - &doStartStopTestUser); - TouchButtonStartStopUserAutonomousDrive.setCaptionForValueTrue(F("Stop\nUser")); + TouchButtonFollower.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, + COLOR16_RED, F("Start\nFollow"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, (sDriveMode == MODE_FOLLOWER), + &doStartStopFollowerMode); + TouchButtonFollower.setCaptionForValueTrue(F("Stop\nFollow")); #ifdef ENABLE_PATH_INFO_PAGE - TouchButtonPathInfoPage.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR_RED, F("Show\nPath"), TEXT_SIZE_22, + TouchButtonPathInfoPage.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR16_RED, F("Show\nPath"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_SHOW_PATH, &GUISwitchPages); #endif } @@ -254,7 +255,7 @@ void stopAutonomousDrivePage(void) { */ void clearPrintedForwardDistancesInfos() { BlueDisplay1.fillRect(BUTTON_WIDTH_3_5 + 1, BUTTON_HEIGHT_4 + 1, LAYOUT_320_WIDTH, - BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER) - 1, COLOR_WHITE); + BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER) - 1, COLOR16_WHITE); } /* @@ -273,15 +274,15 @@ void drawForwardDistancesInfos() { * Determine color */ uint8_t tDistance = sForwardDistancesInfo.RawDistancesArray[i]; - tColor = COLOR_ORANGE; + tColor = COLOR16_ORANGE; if (tDistance >= DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE) { tDistance = DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE; - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } if (tDistance > sCentimeterPerScanTimesTwo) { - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } else if (tDistance < sCentimeterPerScan) { - tColor = COLOR_RED; + tColor = COLOR16_RED; } /* @@ -298,15 +299,15 @@ void drawForwardDistancesInfos() { void drawCollisionDecision(int aDegreeToTurn, uint8_t aLengthOfVector, bool aDoClearVector) { if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - color16_t tColor = COLOR_BLUE; + color16_t tColor = COLOR16_BLUE; int tDegreeToDisplay = aDegreeToTurn; if (tDegreeToDisplay == 180) { - tColor = COLOR_RED; + tColor = COLOR16_RED; tDegreeToDisplay = 0; } if (aDoClearVector) { - tColor = COLOR_WHITE; + tColor = COLOR16_WHITE; } BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, aLengthOfVector, tDegreeToDisplay + 90, @@ -315,7 +316,7 @@ void drawCollisionDecision(int aDegreeToTurn, uint8_t aLengthOfVector, bool aDoC sprintf_P(sStringBuffer, PSTR("wall%4d\xB0 rotation: %3d\xB0 wall%4d\xB0"), sForwardDistancesInfo.WallLeftAngleDegrees, aDegreeToTurn, sForwardDistancesInfo.WallRightAngleDegrees); // \xB0 is degree character BlueDisplay1.drawText(BUTTON_WIDTH_3_5_POS_2, US_DISTANCE_MAP_ORIGIN_Y + TEXT_SIZE_11, sStringBuffer, TEXT_SIZE_11, - COLOR_BLACK, COLOR_WHITE); + COLOR16_BLACK, COLOR16_WHITE); } } } diff --git a/src/BTSensorDrivePage.cpp b/src/BTSensorDrivePage.cpp index 5419664..f6edd6d 100644 --- a/src/BTSensorDrivePage.cpp +++ b/src/BTSensorDrivePage.cpp @@ -23,26 +23,25 @@ #include "RobotCar.h" #include "RobotCarGui.h" -//#pragma GCC diagnostic ignored "-Wunused-parameter" - // 4 Sliders for accelerometer BDSlider SliderForward; // Y negative BDSlider SliderBackward; // Y positive BDSlider SliderRight; // X negative BDSlider SliderLeft; // X positive -#define SLIDER_BACKGROUND_COLOR COLOR_YELLOW -#define SLIDER_BAR_COLOR COLOR_GREEN -#define SLIDER_THRESHOLD_COLOR COLOR_BLUE +#define SLIDER_BACKGROUND_COLOR COLOR16_YELLOW +#define SLIDER_BAR_COLOR COLOR16_GREEN +#define SLIDER_THRESHOLD_COLOR COLOR16_BLUE #define SENSOR_SLIDER_WIDTH (DISPLAY_WIDTH / 16) #define VERTICAL_SLIDER_LENTGH ((DISPLAY_HEIGHT / 4) + (DISPLAY_HEIGHT / 10)) -#define SLIDER_SPEED_THRESHOLD DEFAULT_START_SPEED -#define SPEED_DEAD_BAND (DEFAULT_START_SPEED / 2) +#define SLIDER_SPEED_THRESHOLD DEFAULT_START_SPEED_PWM +#define SPEED_SENSOR_DEAD_BAND 20 +#define SPEED_DEAD_BAND (DEFAULT_START_SPEED_PWM / 2) #define HORIZONTAL_SLIDER_LENTGH (DISPLAY_HEIGHT / 4) #define SLIDER_LEFT_RIGHT_THRESHOLD (HORIZONTAL_SLIDER_LENTGH / 4) -#define LEFT_RIGHT_DEAD_BAND 4 +#define LEFT_RIGHT_SENSOR_DEAD_BAND 10 #define SENSOR_SLIDER_CENTER_X ((DISPLAY_WIDTH - SENSOR_SLIDER_WIDTH) / 2) #define SENSOR_SLIDER_CENTER_Y ((DISPLAY_HEIGHT / 2) + (DISPLAY_HEIGHT / 16)) @@ -55,18 +54,72 @@ uint8_t sSensorChangeCallCountForZeroAdjustment; float sYZeroValueAdded; // The accumulator for the values of the first 8 calls. float sYZeroValue = 0; -int sLastSpeedValue = 0; -int sLastLeftRightValue = 0; +#if (VERSION_BLUE_DISPLAY_MAJOR <= 2) && (VERSION_BLUE_DISPLAY_MINOR <= 1) +// positiveNegativeSlider is available since 2.2 +/* + * To show a signed value on two sliders positioned back to back (one of it is inverse or has a negative length value) + */ +struct positiveNegativeSlider { + BDSlider *positiveSliderPtr; + BDSlider *negativeSliderPtr; + int lastSliderValue; // positive value with sensor dead band applied + BDSlider *lastZeroSlider; // to decide if we draw new zero slider +}; +/* + * @return aValue with aSliderDeadBand applied + */ +int setPositiveNegativeSliders(struct positiveNegativeSlider *aSliderStructPtr, int aValue, uint8_t aSliderDeadBand) { + BDSlider *tValueSlider = aSliderStructPtr->positiveSliderPtr; + BDSlider *tZeroSlider = aSliderStructPtr->negativeSliderPtr; + if (aValue < 0) { + aValue = -aValue; + tValueSlider = tZeroSlider; + tZeroSlider = aSliderStructPtr->positiveSliderPtr; + } -uint8_t speedOverflowAndDeadBandHandling(unsigned int aSpeed) { - // overflow handling since analogWrite only accepts byte values - if (aSpeed > 0xFF) { - aSpeed = 0xFF; + /* + * Now we have a positive value for dead band and slider length + */ + if (aValue > aSliderDeadBand) { + // dead band subtraction -> resulting values start at 0 + aValue -= aSliderDeadBand; + } else { + aValue = 0; } - if (aSpeed <= SPEED_DEAD_BAND) { - aSpeed = 0; + + /* + * Draw slider value if values changed + */ + if (aSliderStructPtr->lastSliderValue != aValue) { + aSliderStructPtr->lastSliderValue = aValue; + tValueSlider->setValueAndDrawBar(aValue); + if (aSliderStructPtr->lastZeroSlider != tZeroSlider) { + aSliderStructPtr->lastZeroSlider = tZeroSlider; + // the sign has changed, clear old value + tZeroSlider->setValueAndDrawBar(0); + } + } + + /* + * Restore sign for aValue with dead band applied + */ + if (tZeroSlider == aSliderStructPtr->positiveSliderPtr) { + aValue = -aValue; } - return aSpeed; + return aValue; +} +#endif + +struct positiveNegativeSlider sAccelerationLeftRightSliders; +struct positiveNegativeSlider sAccelerationForwardBackwardSliders; + +uint8_t speedOverflowAndDeadBandHandling(unsigned int aSpeedPWM) { + aSpeedPWM += SPEED_DEAD_BAND; + // overflow handling since analogWrite only accepts byte values + if (aSpeedPWM > MAX_SPEED_PWM) { + aSpeedPWM = MAX_SPEED_PWM; + } + return aSpeedPWM; } /* @@ -79,13 +132,15 @@ uint8_t speedOverflowAndDeadBandHandling(unsigned int aSpeed) { * positive -> left down * negative -> right down */ -void doSensorChange(uint8_t aSensorType, struct SensorCallback * aSensorCallbackInfo) { +void doSensorChange(uint8_t aSensorType, struct SensorCallback *aSensorCallbackInfo) { + (void) aSensorType; // to avoid -Wunused-parameter + if (sSensorChangeCallCountForZeroAdjustment < CALLS_FOR_ZERO_ADJUSTMENT) { if (sSensorChangeCallCountForZeroAdjustment == 0) { // init values sYZeroValueAdded = 0; } - // Add for zero adjustment + // Sum for zero adjustment sYZeroValueAdded += aSensorCallbackInfo->ValueY; sSensorChangeCallCountForZeroAdjustment++; } else if (sSensorChangeCallCountForZeroAdjustment == CALLS_FOR_ZERO_ADJUSTMENT) { @@ -95,69 +150,43 @@ void doSensorChange(uint8_t aSensorType, struct SensorCallback * aSensorCallback sYZeroValue = sYZeroValueAdded / CALLS_FOR_ZERO_ADJUSTMENT; BlueDisplay1.playTone(24); // feedback for zero value acquired } else { + /* * regular operation here * left right handling */ - // Scale value - int tLeftRightValue = aSensorCallbackInfo->ValueX * 8.0; - - BDSlider * tValueSlider = &SliderLeft; - BDSlider * tZeroSlider = &SliderRight; - if (tLeftRightValue < 0) { - tLeftRightValue = -tLeftRightValue; - tValueSlider = &SliderRight; - tZeroSlider = &SliderLeft; - } - if (tLeftRightValue > LEFT_RIGHT_DEAD_BAND) { - // dead band subtraction - tLeftRightValue -= LEFT_RIGHT_DEAD_BAND; - } else { - tLeftRightValue = 0; - } +#ifdef CAR_HAS_4_WHEELS + int tLeftRightValue = aSensorCallbackInfo->ValueX * 12.0; +#else + int tLeftRightValue = aSensorCallbackInfo->ValueX * 8.0; // Scale value +#endif + tLeftRightValue = setPositiveNegativeSliders(&sAccelerationLeftRightSliders, tLeftRightValue, LEFT_RIGHT_SENSOR_DEAD_BAND); - if (sLastLeftRightValue != tLeftRightValue) { - sLastLeftRightValue = tLeftRightValue; - - tValueSlider->setValueAndDrawBar(tLeftRightValue); - tZeroSlider->setValueAndDrawBar(0); - } + /* + * forward backward handling + */ + int tSpeedPWMValue = -((aSensorCallbackInfo->ValueY - sYZeroValue) * (MAX_SPEED_PWM / 10)); // Scale value + tSpeedPWMValue = setPositiveNegativeSliders(&sAccelerationForwardBackwardSliders, tSpeedPWMValue, SPEED_SENSOR_DEAD_BAND); /* - * restore sign for speed setting below + * Print speed as value of bottom slider */ - if (tZeroSlider == &SliderLeft) { - tLeftRightValue = -tLeftRightValue; - } + sprintf(sStringBuffer, "%4d", tSpeedPWMValue); + SliderBackward.printValue(sStringBuffer); /* - * forward backward handling + * Get direction */ - int tSpeedValue = -((aSensorCallbackInfo->ValueY - sYZeroValue) * (MAX_SPEED / 7)); - if (sLastSpeedValue != tSpeedValue) { - sLastSpeedValue = tSpeedValue; - if (tSpeedValue >= 0) { - // Forward - tSpeedValue = speedOverflowAndDeadBandHandling(tSpeedValue); - - RobotCarMotorControl.setSpeedCompensated(tSpeedValue, DIRECTION_FORWARD, tLeftRightValue); - SliderBackward.setValueAndDrawBar(0); - SliderForward.setValueAndDrawBar(tSpeedValue); - - } else { - // Backward - tSpeedValue = speedOverflowAndDeadBandHandling(-tSpeedValue); - - RobotCarMotorControl.setSpeedCompensated(tSpeedValue, DIRECTION_BACKWARD, tLeftRightValue); - SliderForward.setValueAndDrawBar(0); - SliderBackward.setValueAndDrawBar(tSpeedValue); - } - /* - * Print speed as value of bottom slider - */ - sprintf(sStringBuffer, "%3d", tSpeedValue); - SliderBackward.printValue(sStringBuffer); + uint8_t tDirection = DIRECTION_FORWARD; + if (tSpeedPWMValue < 0) { + tSpeedPWMValue = -tSpeedPWMValue; + tDirection = DIRECTION_BACKWARD; } + + RobotCarMotorControl.rightCarMotor.setSpeedPWMCompensated(speedOverflowAndDeadBandHandling(tSpeedPWMValue + tLeftRightValue), + tDirection); + RobotCarMotorControl.leftCarMotor.setSpeedPWMCompensated(speedOverflowAndDeadBandHandling(tSpeedPWMValue - tLeftRightValue), + tDirection); } } @@ -176,8 +205,10 @@ void initBTSensorDrivePage(void) { SliderBackward.init(SENSOR_SLIDER_CENTER_X, SENSOR_SLIDER_CENTER_Y, SENSOR_SLIDER_WIDTH, -(VERTICAL_SLIDER_LENTGH), SLIDER_SPEED_THRESHOLD, 0, SLIDER_BACKGROUND_COLOR, SLIDER_BAR_COLOR, FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); // SliderBackward.setBarThresholdColor(SLIDER_THRESHOLD_COLOR); - SliderForward.setScaleFactor((float) MAX_SPEED / (float) VERTICAL_SLIDER_LENTGH); - SliderBackward.setScaleFactor((float) MAX_SPEED / (float) VERTICAL_SLIDER_LENTGH); + SliderForward.setScaleFactor((float) MAX_SPEED_PWM / (float) VERTICAL_SLIDER_LENTGH); + SliderBackward.setScaleFactor((float) MAX_SPEED_PWM / (float) VERTICAL_SLIDER_LENTGH); + sAccelerationForwardBackwardSliders.positiveSliderPtr = &SliderForward; + sAccelerationForwardBackwardSliders.negativeSliderPtr = &SliderBackward; // Position slider right from at middle of screen SliderRight.init(SENSOR_SLIDER_CENTER_X + SENSOR_SLIDER_WIDTH, SENSOR_SLIDER_CENTER_Y - (SENSOR_SLIDER_WIDTH / 2), @@ -192,6 +223,8 @@ void initBTSensorDrivePage(void) { // SliderLeft.setBarThresholdColor(SLIDER_THRESHOLD_COLOR); SliderRight.setScaleFactor(1.2); SliderLeft.setScaleFactor(1.2); + sAccelerationLeftRightSliders.positiveSliderPtr = &SliderLeft; + sAccelerationLeftRightSliders.negativeSliderPtr = &SliderRight; } void drawBTSensorDrivePage(void) { @@ -207,6 +240,7 @@ void drawBTSensorDrivePage(void) { } void startBTSensorDrivePage(void) { + doReset(NULL, 0); drawBTSensorDrivePage(); } diff --git a/src/CarMotorControl.cpp b/src/CarMotorControl.cpp deleted file mode 100644 index 6775907..0000000 --- a/src/CarMotorControl.cpp +++ /dev/null @@ -1,615 +0,0 @@ -/* - * CarMotorControl.cpp - * - * Contains functions for control of the 2 motors of a car like setDirection, goDistanceCentimeter() and rotateCar(). - * Checks input of PIN aPinFor2WDDetection since we need different factors for rotating a 4 wheel and a 2 wheel car. - * - * Requires EncoderMotor.cpp - * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer - * armin.joachimsmeyer@gmail.com - * - * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#include -#include "CarMotorControl.h" - -//#define DEBUG // Only for development - -CarMotorControl * sCarMotorControlPointerForISR; - -CarMotorControl::CarMotorControl() { // @suppress("Class members should be properly initialized") - sCarMotorControlPointerForISR = this; -} - -#ifdef USE_ADAFRUIT_MOTOR_SHIELD -void CarMotorControl::init(bool aReadFromEeprom) { - leftCarMotor.init(1, aReadFromEeprom); - rightCarMotor.init(2, aReadFromEeprom); - -#if defined(CAR_HAS_4_WHEELS) - FactorDegreeToCount = FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT; -#else - FactorDegreeToCount = FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT; -#endif - -# ifdef USE_ENCODER_MOTOR_CONTROL - /* - * For slot type optocoupler interrupts on pin PD2 + PD3 - */ - EncoderMotor::enableINT0AndINT1Interrupts(); -# endif -} - -#else -void CarMotorControl::init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, - uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, bool aReadFromEeprom) { - if (aReadFromEeprom) { - leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin, 1); - rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin, 2); - } else { - leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin); - rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin); - } - - FactorDegreeToCount = FACTOR_DEGREE_TO_COUNT_DEFAULT; - -# ifdef USE_ENCODER_MOTOR_CONTROL - /* - * For slot type optocoupler interrupts on pin PD2 + PD3 - */ - EncoderMotor::enableINT0AndINT1Interrupts(); -# endif -} -#endif - -/* - * Sets default values for min and max speed, factor for distance to time conversion for non encoder motors and reset speed compensation - * Is called automatically at init if parameter aReadFromEeprom is set to false - */ -void CarMotorControl::setDefaultsForFixedDistanceDriving() { - rightCarMotor.setDefaultsForFixedDistanceDriving(); - leftCarMotor.setDefaultsForFixedDistanceDriving(); -} - -/** - * @param aSpeedCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. - * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. - */ -void CarMotorControl::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, int8_t aSpeedCompensationRight) { - if (aSpeedCompensationRight > 0) { - rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, aSpeedCompensationRight); - leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, 0); - } else { - rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, 0); - leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeed, aDriveSpeed, -aSpeedCompensationRight); - } -} - -/** - * @param aSpeedCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. - * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. - */ -void CarMotorControl::setSpeedCompensation(int8_t aSpeedCompensationRight) { - if (aSpeedCompensationRight > 0) { - if (leftCarMotor.SpeedCompensation >= aSpeedCompensationRight) { - leftCarMotor.SpeedCompensation -= aSpeedCompensationRight; - } else { - rightCarMotor.SpeedCompensation += aSpeedCompensationRight; - } - } else { - aSpeedCompensationRight = -aSpeedCompensationRight; - if (rightCarMotor.SpeedCompensation >= aSpeedCompensationRight) { - rightCarMotor.SpeedCompensation -= aSpeedCompensationRight; - } else { - leftCarMotor.SpeedCompensation += aSpeedCompensationRight; - } - } - PWMDcMotor::MotorValuesHaveChanged = true; -} - -void CarMotorControl::setDriveSpeed(uint8_t aDriveSpeed) { - rightCarMotor.setDriveSpeed(aDriveSpeed); - leftCarMotor.setDriveSpeed(aDriveSpeed); -} - -/* - * @return true if direction has changed and motor has stopped - */ -bool CarMotorControl::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { - bool tReturnValue = false; - uint8_t tRequestedDirection = aRequestedDirection & DIRECTION_MASK; - if (CarDirectionOrBrakeMode != tRequestedDirection) { -#ifdef DEBUG - Serial.print(F("Motor mode change to ")); - Serial.println(tRequestedDirection); -#endif - uint8_t tMaxSpeed = max(rightCarMotor.CurrentSpeed, leftCarMotor.CurrentSpeed); - if (tMaxSpeed > 0) { - /* - * Direction change requested but motor still running-> first stop motor - */ -#ifdef DEBUG - Serial.println(F("First stop motor and wait")); -#endif - stopMotors(MOTOR_BRAKE); - delay(tMaxSpeed / 2); // to let motors stop - tReturnValue = true; - } - CarDirectionOrBrakeMode = tRequestedDirection; // The only statement which changes CarDirectionOrBrakeMode to DIRECTION_FORWARD or DIRECTION_BACKWARD - } - return tReturnValue; -} - -/* - * Direct motor control, no state or flag handling - */ -void CarMotorControl::setSpeed(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.setSpeed(aRequestedSpeed, aRequestedDirection); - leftCarMotor.setSpeed(aRequestedSpeed, aRequestedDirection); -} - -/* - * Sets speed adjusted by current compensation value and handle motor state and flags - */ -void CarMotorControl::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); - leftCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); -} - -/* - * Sets speed adjusted by current compensation value and handle motor state and flags - * @param aLeftRightSpeed if positive, this value is subtracted from the left motor value, if negative subtracted from the right motor value - * - */ -void CarMotorControl::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection, int8_t aLeftRightSpeed) { - checkAndHandleDirectionChange(aRequestedDirection); -#ifdef USE_ENCODER_MOTOR_CONTROL - EncoderMotor * tMotorWithModifiedSpeed; -#else - PWMDcMotor * tMotorWithModifiedSpeed; -#endif - if (aLeftRightSpeed >= 0) { - rightCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); - tMotorWithModifiedSpeed = &leftCarMotor; - } else { - aLeftRightSpeed = -aLeftRightSpeed; - leftCarMotor.setSpeedCompensated(aRequestedSpeed, aRequestedDirection); - tMotorWithModifiedSpeed = &rightCarMotor; - } - - if (aRequestedSpeed >= aLeftRightSpeed) { - tMotorWithModifiedSpeed->setSpeedCompensated(aRequestedSpeed - aLeftRightSpeed, aRequestedDirection); - } else { - tMotorWithModifiedSpeed->setSpeedCompensated(0, aRequestedDirection); - } -} - -/* - * Direct motor control, no state or flag handling - */ -void CarMotorControl::setSpeed(int aRequestedSpeed) { - rightCarMotor.setSpeed(aRequestedSpeed); - leftCarMotor.setSpeed(aRequestedSpeed); -} - -/* - * Sets signed speed adjusted by current compensation value and handle motor state and flags - */ -void CarMotorControl::setSpeedCompensated(int aRequestedSpeed) { - rightCarMotor.setSpeedCompensated(aRequestedSpeed); - leftCarMotor.setSpeedCompensated(aRequestedSpeed); -} - -uint8_t CarMotorControl::getCarDirectionOrBrakeMode() { - return CarDirectionOrBrakeMode;; -} - -void CarMotorControl::writeMotorvaluesToEeprom(){ - rightCarMotor.writeMotorvaluesToEeprom(); - leftCarMotor.writeMotorvaluesToEeprom(); -} - -/* - * Stop car - * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE - */ -void CarMotorControl::stopMotors(uint8_t aStopMode) { - CarDirectionOrBrakeMode = aStopMode; - rightCarMotor.stop(aStopMode); - leftCarMotor.stop(aStopMode); -} - -/* - * @param aStopMode MOTOR_BRAKE or MOTOR_RELEASE - */ -void CarMotorControl::setStopMode(uint8_t aStopMode) { - rightCarMotor.setStopMode(aStopMode); - leftCarMotor.setStopMode(aStopMode); -} - -/* - * Stop car and reset all control values as speed, distances, debug values etc. to 0x00 - */ -void CarMotorControl::resetControlValues() { -#ifdef USE_ENCODER_MOTOR_CONTROL - rightCarMotor.resetControlValues(); - leftCarMotor.resetControlValues(); -#endif -} - -/* - * If motor is accelerating or decelerating then updateMotor needs to be called at a fast rate otherwise it will not work correctly - * Used to suppress time consuming display of motor values - */ -bool CarMotorControl::isStateRamp() { -#ifdef SUPPORT_RAMP_UP - return (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP - || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP); -#else - return false; -#endif -} - -/* - * @return true if not stopped (motor expects another update) - */ -bool CarMotorControl::updateMotors() { - bool tMotorsNotStopped = rightCarMotor.updateMotor(); - tMotorsNotStopped |= leftCarMotor.updateMotor(); - return tMotorsNotStopped; -} - -void CarMotorControl::delayAndUpdateMotors(unsigned int aDelayMillis) { - uint32_t tStartMillis = millis(); - do { - updateMotors(); - } while (millis() - tStartMillis <= aDelayMillis); -} - -#ifdef SUPPORT_RAMP_UP -void CarMotorControl::startRampUp(uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startRampUp(aRequestedDirection); - leftCarMotor.startRampUp(aRequestedDirection); -} - -void CarMotorControl::startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startRampUp(aRequestedSpeed, aRequestedDirection); - leftCarMotor.startRampUp(aRequestedSpeed, aRequestedDirection); -} - -/* - * Blocking wait until both motors are at drive speed. 256 milliseconds for ramp up. - */ -void CarMotorControl::waitForDriveSpeed(void (*aLoopCallback)(void)) { - - bool tMotorsNotStopped; - do { - if (aLoopCallback != NULL) { - aLoopCallback(); - } - tMotorsNotStopped = rightCarMotor.updateMotor(); - tMotorsNotStopped |= leftCarMotor.updateMotor(); - } while (tMotorsNotStopped - && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE_SPEED)); -} -#endif - -/* - * If ramp up is not supported, this functions just sets the speed and returns immediately. - * 256 milliseconds for ramp up. - */ -void CarMotorControl::startRampUpAndWait(uint8_t aRequestedSpeed, uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { -#ifdef SUPPORT_RAMP_UP - startRampUp(aRequestedSpeed, aRequestedDirection); - waitForDriveSpeed(aLoopCallback); -#else - checkAndHandleDirectionChange(aRequestedDirection); - (void) aLoopCallback; - rightCarMotor.setSpeedCompensated(aDriveSpeed, aRequestedDirection); - leftCarMotor.setSpeedCompensated(aDriveSpeed, aRequestedDirection); -#endif -} - -void CarMotorControl::startRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { -#ifdef SUPPORT_RAMP_UP - startRampUp(aRequestedDirection); - waitForDriveSpeed(aLoopCallback); -#else - (void) aLoopCallback; - rightCarMotor.setSpeedCompensated(rightCarMotor.DriveSpeed, aRequestedDirection); - leftCarMotor.setSpeedCompensated(leftCarMotor.DriveSpeed, aRequestedDirection); -#endif -} - -/* - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. - * Motor is started by the first call to updateMotors(). - */ -void CarMotorControl::startGoDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, aRequestedDirection); - leftCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, aRequestedDirection); -} - -/* - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. - */ -void CarMotorControl::startGoDistanceCentimeter(uint8_t aRequestedSpeed, unsigned int aDistanceCentimeter, - uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - rightCarMotor.startGoDistanceCount(aRequestedSpeed, aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, - aRequestedDirection); - leftCarMotor.startGoDistanceCount(aRequestedSpeed, aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT, - aRequestedDirection); -} - -void CarMotorControl::goDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection, - void (*aLoopCallback)(void)) { - startGoDistanceCentimeter(aDistanceCentimeter, aRequestedDirection); - waitUntilCarStopped(aLoopCallback); -} - -/* - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. - */ -void CarMotorControl::startGoDistanceCentimeter(int aDistanceCentimeter) { - rightCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); - leftCarMotor.startGoDistanceCount(aDistanceCentimeter * FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); -} -/** - * Wait until distance is reached - * @param aLoopCallback called until car has stopped to avoid blocking - */ -void CarMotorControl::goDistanceCentimeter(int aDistanceCentimeter, void (*aLoopCallback)(void)) { - startGoDistanceCentimeter(aDistanceCentimeter); - waitUntilCarStopped(aLoopCallback); -} - -/* - * Stop car with ramp and give DistanceCountAfterRampUp counts for braking. - */ -void CarMotorControl::stopCarAndWaitForIt(void (*aLoopCallback)(void)) { - if (isStopped()) { - return; - } -#if defined(USE_ENCODER_MOTOR_CONTROL) && defined(SUPPORT_RAMP_UP) - /* - * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN - * Use DistanceCountAfterRampUp as ramp down count - */ - rightCarMotor.NextChangeMaxTargetCount = rightCarMotor.EncoderCount; - rightCarMotor.TargetDistanceCount = rightCarMotor.EncoderCount + rightCarMotor.DistanceCountAfterRampUp; - leftCarMotor.NextChangeMaxTargetCount = leftCarMotor.EncoderCount; - leftCarMotor.TargetDistanceCount = leftCarMotor.EncoderCount + leftCarMotor.DistanceCountAfterRampUp; - /* - * blocking wait for stop - */ - waitUntilCarStopped(aLoopCallback); -#else - (void) aLoopCallback; - rightCarMotor.stop(); - leftCarMotor.stop(); - CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode -#endif -} - -/* - * Wait with optional wait loop callback - */ -void CarMotorControl::waitUntilCarStopped(void (*aLoopCallback)(void)) { - do { - rightCarMotor.updateMotor(); - leftCarMotor.updateMotor(); - if (aLoopCallback != NULL) { - aLoopCallback(); - } - } while (!isStopped()); - CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode -} - -bool CarMotorControl::isState(uint8_t aState) { -#if defined(SUPPORT_RAMP_UP) - return (rightCarMotor.MotorRampState == aState && leftCarMotor.MotorRampState == aState); -#else - (void) aState; - return false; -#endif -} - -bool CarMotorControl::isStopped() { - return (rightCarMotor.CurrentSpeed == 0 && leftCarMotor.CurrentSpeed == 0); -} - -void CarMotorControl::setFactorDegreeToCount(float aFactorDegreeToCount) { - FactorDegreeToCount = aFactorDegreeToCount; -} - -/** - * Set distances and speed for 2 motors to turn the requested angle - * @param aRotationDegrees positive -> turn left, negative -> turn right - * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE - * @param aUseSlowSpeed true -> use slower speed (1.5 times StartSpeed) instead of DriveSpeed for rotation to be more exact - */ -void CarMotorControl::startRotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed) { - int tDistanceCountRight; - int tDistanceCountLeft; - unsigned int tDistanceCount; - - uint8_t tTurnDirectionRightMotor = aTurnDirection; // set turn direction as start direction - uint8_t tTurnDirectionLeftMotor = aTurnDirection; - if (aTurnDirection == TURN_BACKWARD) { - // swap turn sign / (left / right) to move other motor - aRotationDegrees = -aRotationDegrees; - } - - /* - * Here aRotationDegrees is modified for TURN_FORWARD - */ - if (aRotationDegrees > 0) { - tDistanceCount = (aRotationDegrees * FactorDegreeToCount) + 0.5; - /* - * Move right motor (except for TURN_IN_PLACE) - * Here we have a left turn and a direction TURN_FORWARD or TURN_IN_PLACE - * OR a right turn and a direction TURN_BACKWARD - */ - tDistanceCountRight = tDistanceCount; - tDistanceCountLeft = 0; - - if (aTurnDirection == TURN_IN_PLACE) { - tDistanceCountRight /= 2; - tDistanceCountLeft = tDistanceCountRight; - // The right motor has turn direction TURN_IN_PLACE which is masked to TURN_FORWARD by startGoDistanceCount() - tTurnDirectionLeftMotor = TURN_BACKWARD; - } - } else { - tDistanceCount = (-aRotationDegrees * FactorDegreeToCount) + 0.5; - /* - * Move left motor (except for TURN_IN_PLACE) - * Here we have a right turn and a direction TURN_FORWARD or TURN_IN_PLACE - * OR a left turn and a direction TURN_BACKWARD - */ - tDistanceCountLeft = tDistanceCount; - tDistanceCountRight = 0; - if (aTurnDirection == TURN_IN_PLACE) { - tDistanceCountLeft /= 2; - tDistanceCountRight = tDistanceCountLeft; - tTurnDirectionRightMotor = TURN_BACKWARD; - } - } - - // This in turn sets CurrentDriveSpeed to DriveSpeed. - rightCarMotor.startGoDistanceCount(tDistanceCountRight, tTurnDirectionRightMotor); - leftCarMotor.startGoDistanceCount(tDistanceCountLeft, tTurnDirectionLeftMotor); - if (aUseSlowSpeed) { - // adjust CurrentDriveSpeed -#if defined(SUPPORT_RAMP_UP) - // avoid overflow, the reduced speed is almost max speed then. - if (rightCarMotor.StartSpeed < 160) { - rightCarMotor.CurrentDriveSpeed = rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2; - } - if (leftCarMotor.StartSpeed < 160) { - leftCarMotor.CurrentDriveSpeed = leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2; - } -#else - if (tDistanceCountRight > 0) { - rightCarMotor.setSpeedCompensated(rightCarMotor.StartSpeed + rightCarMotor.StartSpeed / 2, - rightCarMotor.CurrentDirectionOrBrakeMode); - } - if (tDistanceCountLeft > 0) { - leftCarMotor.setSpeedCompensated(leftCarMotor.StartSpeed + leftCarMotor.StartSpeed / 2, - leftCarMotor.CurrentDirectionOrBrakeMode); - } -#endif - } -} - -/** - * @param aRotationDegrees positive -> turn left, negative -> turn right - * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE (default) - * @param aUseSlowSpeed true (default) -> use slower speed (1.5 times StartSpeed) instead of DriveSpeed for rotation to be more exact - * @param aLoopCallback avoid blocking and call aLoopCallback on waiting for stop - */ -void CarMotorControl::rotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed, void (*aLoopCallback)(void)) { - if (aRotationDegrees != 0) { - startRotateCar(aRotationDegrees, aTurnDirection, aUseSlowSpeed); - waitUntilCarStopped(aLoopCallback); - } -} - -#ifndef USE_ENCODER_MOTOR_CONTROL -void CarMotorControl::setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor) { - rightCarMotor.setDistanceToTimeFactorForFixedDistanceDriving(aDistanceToTimeFactor); - leftCarMotor.setDistanceToTimeFactorForFixedDistanceDriving(aDistanceToTimeFactor); -} - -#else - -/* - * Get count / distance value from right motor - */ -unsigned int CarMotorControl::getDistanceCount() { - return (rightCarMotor.EncoderCount); -} - -int CarMotorControl::getDistanceCentimeter() { - return (rightCarMotor.EncoderCount / FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT); -} - -/* - * generates a rising ramp and detects the first movement -> this sets dead band / minimum Speed - */ -void CarMotorControl::calibrate() { - stopMotors(); - resetControlValues(); - - rightCarMotor.StartSpeed = 0; - leftCarMotor.StartSpeed = 0; - - uint8_t tMotorMovingCount = 0; - - /* - * increase motor speed by 1 until motor moves - */ - for (uint8_t tSpeed = 20; tSpeed != 0xFF; ++tSpeed) { - if (rightCarMotor.StartSpeed == 0) { - rightCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD); - } - if (leftCarMotor.StartSpeed == 0) { - leftCarMotor.setSpeed(tSpeed, DIRECTION_FORWARD); - } - - delay(100); - /* - * Check if wheel moved - */ - - /* - * Store speed after 6 counts (3cm) - */ - if (rightCarMotor.StartSpeed == 0 && rightCarMotor.EncoderCount > 6) { - rightCarMotor.StartSpeed = tSpeed; - tMotorMovingCount++; - } - if (leftCarMotor.StartSpeed == 0 && leftCarMotor.EncoderCount > 6) { - leftCarMotor.StartSpeed = tSpeed; - tMotorMovingCount++; - } - if (tMotorMovingCount >= 2) { - // Do not end loop if one motor still not moving - break; - } - } - - /* - * TODO calibrate StopSpeed separately - */ - - writeMotorvaluesToEeprom(); - stopMotors(); -} - -// ISR for PIN PD2 / RIGHT -ISR(INT0_vect) { - sCarMotorControlPointerForISR->rightCarMotor.handleEncoderInterrupt(); -} - -// ISR for PIN PD3 / LEFT -ISR(INT1_vect) { - sCarMotorControlPointerForISR->leftCarMotor.handleEncoderInterrupt(); -} -#endif // USE_ENCODER_MOTOR_CONTROL diff --git a/src/CarMotorControl.h b/src/CarMotorControl.h deleted file mode 100644 index 795977c..0000000 --- a/src/CarMotorControl.h +++ /dev/null @@ -1,162 +0,0 @@ -/* - * CarMotorControl.h - * - * Motor control for a car with 2 encoder motors - * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer - * armin.joachimsmeyer@gmail.com - * - * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see . - */ - -#ifndef CARMOTORCONTROL_H_ -#define CARMOTORCONTROL_H_ - -#include "EncoderMotor.h" -#include - -/* - * Some factors depending on wheel diameter and encoder resolution - */ -#if ! defined(FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT) -#define FACTOR_CENTIMETER_TO_COUNT_INTEGER_DEFAULT 2 // Exact value is 1.86, but integer saves program space and time -#endif -// Values for 20 slot encoder discs -> 40 ticks per turn. Circumference of the wheel is 21.5 cm, Distance between two wheels is around 13 cm -#define FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT 0.4277777 -#define FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT 0.8 // estimated, with slip - -#if ! defined(FACTOR_DEGREE_TO_COUNT_DEFAULT) -# if defined(CAR_HAS_4_WHEELS) -#define FACTOR_DEGREE_TO_COUNT_DEFAULT FACTOR_DEGREE_TO_COUNT_4WD_CAR_DEFAULT -# else -#define FACTOR_DEGREE_TO_COUNT_DEFAULT FACTOR_DEGREE_TO_COUNT_2WD_CAR_DEFAULT -# endif -#endif - -// turn directions -#define TURN_FORWARD DIRECTION_FORWARD // 0 -#define TURN_BACKWARD DIRECTION_BACKWARD // 1 -#define TURN_IN_PLACE 2 - -class CarMotorControl { -public: - - CarMotorControl(); -// virtual ~CarMotorControl(); - -#ifdef USE_ADAFRUIT_MOTOR_SHIELD - void init(bool aReadFromEeprom = false); -#else - void init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, uint8_t aLeftMotorForwardPin, - uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, bool aReadFromEeprom = false); -#endif - - void setDefaultsForFixedDistanceDriving(); - void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, int8_t aSpeedCompensationRight); - void setSpeedCompensation(int8_t aSpeedCompensationRight); - void setDriveSpeed(uint8_t aDriveSpeed); - void writeMotorvaluesToEeprom(); - -#ifdef USE_ENCODER_MOTOR_CONTROL - void calibrate(); - // retrieves values from right motor - unsigned int getDistanceCount(); - int getDistanceCentimeter(); -#else - // makes no sense for encoder motor - void setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor); -#endif - -#ifdef SUPPORT_RAMP_UP - void startRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD); - void startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void waitForDriveSpeed(void (*aLoopCallback)(void) = NULL); -#endif - // If ramp up is not supported, these functions just sets the speed and return immediately - void startRampUpAndWait(uint8_t aRequestedSpeed, uint8_t aRequestedDirection = DIRECTION_FORWARD, - void (*aLoopCallback)(void) = NULL); - void startRampUpAndWaitForDriveSpeed(uint8_t aRequestedDirection = DIRECTION_FORWARD, void (*aLoopCallback)(void) = NULL); - - /* - * For car direction handling - */ - uint8_t getCarDirectionOrBrakeMode(); - uint8_t CarDirectionOrBrakeMode; - - /* - * Functions for moving a fixed distance - */ - // With signed distance - void startGoDistanceCentimeter(uint8_t aRequestedSpeed, unsigned int aDistanceCentimeter, uint8_t aRequestedDirection); // only setup values - void startGoDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection); // only setup values - void startGoDistanceCentimeter(int aDistanceCentimeter); // only setup values, no movement -> use updateMotors() - - void goDistanceCentimeter(int aDistanceCentimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilCarStopped - void goDistanceCentimeter(unsigned int aDistanceCentimeter, uint8_t aRequestedDirection, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilCarStopped - - bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); // used internally - - /* - * Functions for rotation - */ - void setFactorDegreeToCount(float aFactorDegreeToCount); - void startRotateCar(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = true); - void rotateCar(int aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, bool aUseSlowSpeed = true, - void (*aLoopCallback)(void) = NULL); - float FactorDegreeToCount; - - bool updateMotors(); - void delayAndUpdateMotors(unsigned int aDelayMillis); - - /* - * Start/Stop functions for infinite distance - */ - void stopCarAndWaitForIt(void (*aLoopCallback)(void) = NULL); // uses waitUntilCarStopped() - void waitUntilCarStopped(void (*aLoopCallback)(void) = NULL); - - /* - * Check motor state functions - */ - bool isStopped(); - bool isState(uint8_t aState); - bool isStateRamp(); // MOTOR_STATE_RAMP_UP OR MOTOR_STATE_RAMP_DOWN - - void resetControlValues(); - - /* - * Functions, which directly call motor functions for both motors - */ - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection, int8_t aLeftRightSpeed); - void stopMotors(uint8_t aStopMode = STOP_MODE_KEEP); - - void setSpeed(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); - void setStopMode(uint8_t aStopMode); - void setSpeed(int aRequestedSpeed); - void setSpeedCompensated(int aRequestedSpeed); - -#ifdef USE_ENCODER_MOTOR_CONTROL - EncoderMotor rightCarMotor; // 40 bytes RAM - EncoderMotor leftCarMotor; -#else - PWMDcMotor rightCarMotor; - PWMDcMotor leftCarMotor; -#endif -}; - -// Pointer to the last and only! instance for use by ISR -extern CarMotorControl * sCarMotorControlPointerForISR; - -#endif /* CARMOTORCONTROL_H_ */ - -#pragma once - diff --git a/src/CarPWMMotorControl.cpp b/src/CarPWMMotorControl.cpp new file mode 100644 index 0000000..98ebbc9 --- /dev/null +++ b/src/CarPWMMotorControl.cpp @@ -0,0 +1,828 @@ +/* + * CarPWMMotorControl.cpp + * + * Contains functions for control of the 2 motors of a car like setDirection, goDistanceMillimeter() and rotate(). + * Checks input of PIN aPinFor2WDDetection since we need different factors for rotating a 4 wheel and a 2 wheel car. + * + * Requires EncoderMotor.cpp + * + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include "CarPWMMotorControl.h" + +#define DEBUG // Only for development + +CarPWMMotorControl::CarPWMMotorControl() { // @suppress("Class members should be properly initialized") +} + +#ifdef USE_MPU6050_IMU +/* + * This must be done when the car is not moving, best after at least 100 ms after boot up. + */ +void CarPWMMotorControl::calculateAndPrintIMUOffsets(Print *aSerial) { + IMUData.calculateSpeedAndTurnOffsets(); + IMUData.printSpeedAndTurnOffsets(aSerial); +} +#endif +/* + * If no parameter and we have encoder motors, we use a fixed assignment of rightCarMotor interrupts to INT0 / Pin2 and leftCarMotor to INT1 / Pin3 + */ +#ifdef USE_ADAFRUIT_MOTOR_SHIELD +void CarPWMMotorControl::init() { +# ifdef USE_ENCODER_MOTOR_CONTROL + leftCarMotor.init(1, INT1); + rightCarMotor.init(2, INT0); +# else + leftCarMotor.init(1); + rightCarMotor.init(2); +# endif + +# ifdef USE_MPU6050_IMU + CarRequestedRotationDegrees = 0; + CarRequestedDistanceMillimeter = 0; + IMUData.initMPU6050FifoForCarData(); +# else +# if defined(CAR_HAS_4_WHEELS) + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_4WD_CAR_DEFAULT; +# else + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_2WD_CAR_DEFAULT; +# endif +# endif +} + +#else // USE_ADAFRUIT_MOTOR_SHIELD +void CarPWMMotorControl::init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, + uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin) { + leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin); + rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin); + +# ifdef USE_MPU6050_IMU + CarRequestedRotationDegrees = 0; + CarRequestedDistanceMillimeter = 0; + IMUData.initMPU6050FifoForCarData(); + +# else + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_DEFAULT; +# endif +# ifdef USE_ENCODER_MOTOR_CONTROL + /* + * For slot type optocoupler interrupts on pin PD2 + PD3 + */ + rightCarMotor.attachInterrupt(INT0); + leftCarMotor.attachInterrupt(INT1); +# endif +} + +# ifdef USE_ENCODER_MOTOR_CONTROL +/* + * With parameters aRightInterruptNumber + aLeftInterruptNumber + */ +void CarPWMMotorControl::init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, + uint8_t aRightInterruptNumber, uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, + uint8_t aLeftInterruptNumber) { + leftCarMotor.init(aLeftMotorForwardPin, LeftMotorBackwardPin, aLeftMotorPWMPin, aLeftInterruptNumber); + rightCarMotor.init(aRightMotorForwardPin, aRightMotorBackwardPin, aRightPWMPin, aRightInterruptNumber); + +# ifdef USE_MPU6050_IMU + CarRequestedRotationDegrees = 0; + CarRequestedDistanceMillimeter = 0; + IMUData.initMPU6050FifoForCarData(); +# else + FactorDegreeToMillimeter = FACTOR_DEGREE_TO_MILLIMETER_DEFAULT; +# endif +} +# endif // USE_ENCODER_MOTOR_CONTROL +#endif // USE_ADAFRUIT_MOTOR_SHIELD + +/* + * Sets default values for min and max speed, factor for distance to time conversion for non encoder motors and reset speed compensation + * Is called automatically at init if parameter aReadFromEeprom is set to false + */ +void CarPWMMotorControl::setDefaultsForFixedDistanceDriving() { + rightCarMotor.setDefaultsForFixedDistanceDriving(); + leftCarMotor.setDefaultsForFixedDistanceDriving(); +} + +/** + * @param aSpeedPWMCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. + * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. + */ +void CarPWMMotorControl::setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, + int8_t aSpeedPWMCompensationRight) { + if (aSpeedPWMCompensationRight >= 0) { + rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, aSpeedPWMCompensationRight); + leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, 0); + } else { + rightCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, 0); + leftCarMotor.setValuesForFixedDistanceDriving(aStartSpeedPWM, aDriveSpeedPWM, -aSpeedPWMCompensationRight); + } +} + +/** + * @param aSpeedPWMCompensationRight if positive, this value is added to the compensation value of the right motor, or subtracted from the left motor value. + * If negative, -value is added to the compensation value the left motor, or subtracted from the right motor value. + */ +void CarPWMMotorControl::changeSpeedPWMCompensation(int8_t aSpeedPWMCompensationRight) { + if (aSpeedPWMCompensationRight > 0) { + if (leftCarMotor.SpeedPWMCompensation >= aSpeedPWMCompensationRight) { + leftCarMotor.SpeedPWMCompensation -= aSpeedPWMCompensationRight; + } else { + rightCarMotor.SpeedPWMCompensation += aSpeedPWMCompensationRight; + } + } else { + aSpeedPWMCompensationRight = -aSpeedPWMCompensationRight; + if (rightCarMotor.SpeedPWMCompensation >= aSpeedPWMCompensationRight) { + rightCarMotor.SpeedPWMCompensation -= aSpeedPWMCompensationRight; + } else { + leftCarMotor.SpeedPWMCompensation += aSpeedPWMCompensationRight; + } + } + PWMDcMotor::MotorControlValuesHaveChanged = true; +} + +void CarPWMMotorControl::setStartSpeedPWM(uint8_t aStartSpeedPWM) { + rightCarMotor.setStartSpeedPWM(aStartSpeedPWM); + leftCarMotor.setStartSpeedPWM(aStartSpeedPWM); +} + +void CarPWMMotorControl::setDriveSpeedPWM(uint8_t aDriveSpeedPWM) { + rightCarMotor.setDriveSpeedPWM(aDriveSpeedPWM); + leftCarMotor.setDriveSpeedPWM(aDriveSpeedPWM); +} + +/* + * @return true if direction has changed and motor has stopped + */ +bool CarPWMMotorControl::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { + bool tReturnValue = false; + if (CarDirectionOrBrakeMode != aRequestedDirection) { + uint8_t tMaxCurrentSpeedPWM = max(rightCarMotor.CurrentSpeedPWM, leftCarMotor.CurrentSpeedPWM); + if (tMaxCurrentSpeedPWM > 0) { + /* + * Direction change requested but motor still running-> first stop motor + */ +#ifdef DEBUG + Serial.println(F("First stop motor and wait")); +#endif + stop(MOTOR_BRAKE); +// delay(((tMaxCurrentSpeedPWM * tMaxCurrentSpeedPWM) >> 8) * 2); // to let motors stop + delay(tMaxCurrentSpeedPWM); // to let motors stop + tReturnValue = true; + } +#ifdef DEBUG + Serial.print(F("Change car mode from ")); + Serial.print(CarDirectionOrBrakeMode); + Serial.print(F(" to ")); + Serial.println(aRequestedDirection); +#endif + CarDirectionOrBrakeMode = aRequestedDirection; // The only statement which changes CarDirectionOrBrakeMode to DIRECTION_FORWARD or DIRECTION_BACKWARD + } + return tReturnValue; +} + +/* + * Direct motor control, no state or flag handling + */ +void CarPWMMotorControl::setSpeedPWM(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.setSpeedPWM(aRequestedSpeedPWM, aRequestedDirection); + leftCarMotor.setSpeedPWM(aRequestedSpeedPWM, aRequestedDirection); +} + +/* + * Sets speed adjusted by current compensation value and keeps direction + */ +void CarPWMMotorControl::changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM) { + rightCarMotor.changeSpeedPWMCompensated(aRequestedSpeedPWM); + leftCarMotor.changeSpeedPWMCompensated(aRequestedSpeedPWM); +} + +/* + * Sets speed adjusted by current compensation value and handle motor state and flags + */ +void CarPWMMotorControl::setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); + leftCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); +} + +/* + * Sets speed adjusted by current compensation value and handle motor state and flags + * @param aLeftRightSpeedPWM if positive, this value is subtracted from the left motor value, if negative subtracted from the right motor value + * + */ +void CarPWMMotorControl::setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection, int8_t aLeftRightSpeedPWM) { + checkAndHandleDirectionChange(aRequestedDirection); +#ifdef USE_ENCODER_MOTOR_CONTROL + EncoderMotor *tMotorWithModifiedSpeedPWM; +#else + PWMDcMotor *tMotorWithModifiedSpeedPWM; +#endif + if (aLeftRightSpeedPWM >= 0) { + rightCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); + tMotorWithModifiedSpeedPWM = &leftCarMotor; + } else { + aLeftRightSpeedPWM = -aLeftRightSpeedPWM; + leftCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); + tMotorWithModifiedSpeedPWM = &rightCarMotor; + } + + if (aRequestedSpeedPWM >= aLeftRightSpeedPWM) { + tMotorWithModifiedSpeedPWM->setSpeedPWMCompensated(aRequestedSpeedPWM - aLeftRightSpeedPWM, aRequestedDirection); + } else { + tMotorWithModifiedSpeedPWM->setSpeedPWMCompensated(0, aRequestedDirection); + } +} + +/* + * Direct motor control, no state or flag handling + */ +void CarPWMMotorControl::setSpeedPWM(int aRequestedSpeedPWM) { + rightCarMotor.setSpeedPWM(aRequestedSpeedPWM); + leftCarMotor.setSpeedPWM(aRequestedSpeedPWM); +} + +/* + * Sets signed SpeedPWM adjusted by current compensation value and handle motor state and flags + */ +void CarPWMMotorControl::setSpeedPWMCompensated(int aRequestedSpeedPWM) { + rightCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM); + leftCarMotor.setSpeedPWMCompensated(aRequestedSpeedPWM); +} + +uint8_t CarPWMMotorControl::getCarDirectionOrBrakeMode() { + return CarDirectionOrBrakeMode;; +} + +void CarPWMMotorControl::readMotorValuesFromEeprom() { + leftCarMotor.readMotorValuesFromEeprom(0); + rightCarMotor.readMotorValuesFromEeprom(1); +} + +void CarPWMMotorControl::writeMotorValuesToEeprom() { + leftCarMotor.writeMotorValuesToEeprom(0); + rightCarMotor.writeMotorValuesToEeprom(1); +} + +/* + * Stop car + * @param aStopMode STOP_MODE_KEEP (take previously defined StopMode) or MOTOR_BRAKE or MOTOR_RELEASE + */ +void CarPWMMotorControl::stop(uint8_t aStopMode) { + rightCarMotor.stop(aStopMode); + leftCarMotor.stop(aStopMode); + CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode, STOP_MODE_KEEP is evaluated here +} + +/* + * @param aStopMode MOTOR_BRAKE or MOTOR_RELEASE + */ +void CarPWMMotorControl::setStopMode(uint8_t aStopMode) { + rightCarMotor.setStopMode(aStopMode); + leftCarMotor.setStopMode(aStopMode); +} + +/* + * Stop car and reset all control values as SpeedPWM, distances, debug values etc. to 0x00 + */ +void CarPWMMotorControl::resetControlValues() { +#ifdef USE_ENCODER_MOTOR_CONTROL + rightCarMotor.resetEncoderControlValues(); + leftCarMotor.resetEncoderControlValues(); +#endif +} + +/* + * If motor is accelerating or decelerating then updateMotor needs to be called at a fast rate otherwise it will not work correctly + * Used to suppress time consuming display of motor values + */ +bool CarPWMMotorControl::isStateRamp() { + return (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP + || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN || leftCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP); +} + +#ifdef USE_MPU6050_IMU +void CarPWMMotorControl::updateIMUData() { + if (IMUData.readCarDataFromMPU6050Fifo()) { + if (IMUData.AcceleratorForwardOffset != 0) { + if (CarTurn2DegreesPerSecondFromIMU != IMUData.getGyroscopePan2DegreePerSecond()) { + CarTurn2DegreesPerSecondFromIMU = IMUData.getGyroscopePan2DegreePerSecond(); +// PWMDcMotor::SensorValuesHaveChanged = true; is not displayed + } + if (CarTurnAngleHalfDegreesFromIMU != IMUData.getTurnAngleHalfDegree()) { + CarTurnAngleHalfDegreesFromIMU = IMUData.getTurnAngleHalfDegree(); + PWMDcMotor::SensorValuesHaveChanged = true; + } + if (CarSpeedCmPerSecondFromIMU != (unsigned int) abs(IMUData.getSpeedCmPerSecond())) { + CarSpeedCmPerSecondFromIMU = abs(IMUData.getSpeedCmPerSecond()); + PWMDcMotor::SensorValuesHaveChanged = true; + } + if (CarDistanceMillimeterFromIMU != (unsigned int) abs(IMUData.getDistanceMillimeter())) { + CarDistanceMillimeterFromIMU = abs(IMUData.getDistanceMillimeter()); + PWMDcMotor::SensorValuesHaveChanged = true; + } + } + } +} +#endif + +/* + * @return true if not stopped (motor expects another update) + */ +#define TURN_OVERRUN_HALF_ANGLE 1 // 1/2 degree overrun after stop(MOTOR_BRAKE) +#define RAMP_DOWN_MILLIMETER 50 +#define STOP_OVERRUN_MILLIMETER 10 // 1 cm overrun after stop(MOTOR_BRAKE) +/* + * If IMU data are available, rotation is always handled here. + * For non encoder motors also distance driving is handled here. + * @return true if not stopped (motor expects another update) + */ +bool CarPWMMotorControl::updateMotors() { +#ifdef USE_MPU6050_IMU + bool tReturnValue = true; + updateIMUData(); + if (CarRequestedRotationDegrees != 0) { + /* + * Using ramps for the rotation SpeedPWMs used makes no sense + */ +# ifdef TRACE + Serial.println(CarTurnAngleHalfDegreesFromIMU); + delay(10); +# endif + // putting abs(CarTurnAngleHalfDegreesFromIMU) also into a variable increases code size by 8 + int tRequestedRotationDegreesForCompare = abs(CarRequestedRotationDegrees * 2); + int tCarTurnAngleHalfDegreesFromIMUForCompare = abs(CarTurnAngleHalfDegreesFromIMU); + if ((tCarTurnAngleHalfDegreesFromIMUForCompare + TURN_OVERRUN_HALF_ANGLE) >= tRequestedRotationDegreesForCompare) { + stop(MOTOR_BRAKE); + CarRequestedRotationDegrees = 0; + tReturnValue = false; + } else if ((tCarTurnAngleHalfDegreesFromIMUForCompare + getTurnDistanceHalfDegree()) + >= tRequestedRotationDegreesForCompare) { + Serial.print(getTurnDistanceHalfDegree()); + // Reduce SpeedPWM just before target angle is reached if motors are not stopped we run for extra 2 to 4 degree + changeSpeedPWMCompensated(rightCarMotor.StartSpeedPWM); + } + } else { + if (CarRequestedDistanceMillimeter != 0) { +# ifndef USE_ENCODER_MOTOR_CONTROL + if (rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_UP || rightCarMotor.MotorRampState == MOTOR_STATE_DRIVE + || rightCarMotor.MotorRampState == MOTOR_STATE_RAMP_DOWN) { + unsigned int tBrakingDistanceMillimeter = getBrakingDistanceMillimeter(); +#ifdef DEBUG + Serial.print(F("Dist=")); + Serial.print(CarDistanceMillimeterFromIMU); + Serial.print(F(" Breakdist=")); + Serial.print(tBrakingDistanceMillimeter); + Serial.print(F(" St=")); + Serial.print(rightCarMotor.MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(rightCarMotor.CurrentSpeedPWM); +#endif + if (CarDistanceMillimeterFromIMU >= CarRequestedDistanceMillimeter) { + CarRequestedDistanceMillimeter = 0; + stop(MOTOR_BRAKE); + } + // Transition criteria to brake/ramp down is: Target distance - braking distance reached + if (rightCarMotor.MotorRampState != MOTOR_STATE_RAMP_DOWN + && (CarDistanceMillimeterFromIMU + tBrakingDistanceMillimeter) >= CarRequestedDistanceMillimeter) { + // Start braking + startRampDown(); + } + } +# endif // USE_ENCODER_MOTOR_CONTROL + } + /* + * In case of IMU distance driving only ramp up and down are managed by these calls + */ + tReturnValue = rightCarMotor.updateMotor(); + tReturnValue |= leftCarMotor.updateMotor(); + } + +#else // USE_MPU6050_IMU +bool tReturnValue = rightCarMotor.updateMotor(); +tReturnValue |= leftCarMotor.updateMotor(); +#endif // USE_MPU6050_IMU + + return tReturnValue;; +} + +/* + * @return true if not stopped (motor expects another update) + */ +bool CarPWMMotorControl::updateMotors(void (*aLoopCallback)(void)) { + if (aLoopCallback != NULL) { + aLoopCallback(); + } + return updateMotors(); +} + +void CarPWMMotorControl::delayAndUpdateMotors(unsigned int aDelayMillis) { + uint32_t tStartMillis = millis(); + do { + updateMotors(); + } while (millis() - tStartMillis <= aDelayMillis); +} + +void CarPWMMotorControl::startRampUp(uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.startRampUp(aRequestedDirection); + leftCarMotor.startRampUp(aRequestedDirection); +} + +void CarPWMMotorControl::startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { + checkAndHandleDirectionChange(aRequestedDirection); + rightCarMotor.startRampUp(aRequestedSpeedPWM, aRequestedDirection); + leftCarMotor.startRampUp(aRequestedSpeedPWM, aRequestedDirection); +} + +/* + * Blocking wait until both motors are at drive SpeedPWM. 256 milliseconds for ramp up. + */ +void CarPWMMotorControl::waitForDriveSpeedPWM(void (*aLoopCallback)(void)) { + while (updateMotors(aLoopCallback) + && (rightCarMotor.MotorRampState != MOTOR_STATE_DRIVE || leftCarMotor.MotorRampState != MOTOR_STATE_DRIVE)) { + ; + } +} + +/* + * If ramp up is not supported, this functions just sets the SpeedPWM and returns immediately. + * 256 milliseconds for ramp up. + */ +void CarPWMMotorControl::startRampUpAndWait(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { + startRampUp(aRequestedSpeedPWM, aRequestedDirection); + waitForDriveSpeedPWM(aLoopCallback); +} + +void CarPWMMotorControl::startRampUpAndWaitForDriveSpeedPWM(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { + startRampUp(aRequestedDirection); + waitForDriveSpeedPWM(aLoopCallback); +} + +void CarPWMMotorControl::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +} + +/* + * initialize motorInfo fields LastDirection and CurrentSpeedPWM + */ +void CarPWMMotorControl::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + + checkAndHandleDirectionChange(aRequestedDirection); + +#ifdef USE_MPU6050_IMU + IMUData.resetCarData(); + CarRequestedDistanceMillimeter = aRequestedDistanceMillimeter; +#endif + +#if defined(USE_MPU6050_IMU) && !defined(USE_ENCODER_MOTOR_CONTROL) + // for non encoder motor we use the IMU distance, and require only the ramp up + startRampUp(aRequestedSpeedPWM, aRequestedDirection); +#else + rightCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + leftCarMotor.startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); +#endif +} + +void CarPWMMotorControl::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, + void (*aLoopCallback)(void)) { + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); + waitUntilStopped(aLoopCallback); +} + +void CarPWMMotorControl::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); + } else { + startGoDistanceMillimeter(rightCarMotor.DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); + } +} + +/** + * Wait until distance is reached + * @param aLoopCallback called until car has stopped to avoid blocking + */ +void CarPWMMotorControl::goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)) { + startGoDistanceMillimeter(aRequestedDistanceMillimeter); + waitUntilStopped(aLoopCallback); +} + +/* + * Stop car with ramp and give DistanceCountAfterRampUp counts for braking. + */ +void CarPWMMotorControl::stopAndWaitForIt(void (*aLoopCallback)(void)) { + if (isStopped()) { + return; + } + + rightCarMotor.startRampDown(); + leftCarMotor.startRampDown(); + /* + * blocking wait for stop + */ + waitUntilStopped(aLoopCallback); +} + +void CarPWMMotorControl::startRampDown() { + if (isStopped()) { + return; + } + /* + * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE to MOTOR_STATE_RAMP_DOWN + * Use DistanceCountAfterRampUp as ramp down count + */ + rightCarMotor.startRampDown(); + leftCarMotor.startRampDown(); +} + +/* + * Wait with optional wait loop callback + */ +void CarPWMMotorControl::waitUntilStopped(void (*aLoopCallback)(void)) { + while (updateMotors(aLoopCallback)) { + ; + } + CarDirectionOrBrakeMode = rightCarMotor.CurrentDirectionOrBrakeMode; // get right stopMode +} + +bool CarPWMMotorControl::isState(uint8_t aState) { + return (rightCarMotor.MotorRampState == aState && leftCarMotor.MotorRampState == aState); +} + +bool CarPWMMotorControl::isStopped() { + return (rightCarMotor.CurrentSpeedPWM == 0 && leftCarMotor.CurrentSpeedPWM == 0); +} + +void CarPWMMotorControl::setFactorDegreeToMillimeter(float aFactorDegreeToMillimeter) { +#ifndef USE_MPU6050_IMU + FactorDegreeToMillimeter = aFactorDegreeToMillimeter; +#else + (void) aFactorDegreeToMillimeter; +#endif +} + +/** + * Set distances and SpeedPWM for 2 motors to turn the requested angle + * @param aRotationDegrees positive -> turn left, negative -> turn right + * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE + * @param aUseSlowSpeed true -> use slower SpeedPWM (1.5 times StartSpeedPWM) instead of DriveSpeedPWM for rotation to be more exact + */ +void CarPWMMotorControl::startRotate(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed) { + /* + * We have 6 cases + * - aTurnDirection = TURN_FORWARD + -> left, right motor F, left 0 - -> right, right motor 0, left F + * - aTurnDirection = TURN_BACKWARD + -> left, right motor 0, left B - -> right, right motor B, left 0 + * - aTurnDirection = TURN_IN_PLACE + -> left, right motor F, left B - -> right, right motor B, left F + * Turn direction TURN_IN_PLACE is masked to TURN_FORWARD + */ + +#ifdef DEBUG + Serial.print(F("RotationDegrees=")); + Serial.print(aRotationDegrees); + Serial.print(F(" TurnDirection=")); + Serial.println(aTurnDirection); + Serial.flush(); +#endif + +#ifdef USE_MPU6050_IMU + IMUData.resetCarData(); + CarRequestedRotationDegrees = aRotationDegrees; +#endif + + /* + * Handle positive and negative rotation degrees + */ +#ifdef USE_ENCODER_MOTOR_CONTROL + EncoderMotor *tRightMotorIfPositiveTurn; + EncoderMotor *tLeftMotorIfPositiveTurn; +#else + PWMDcMotor *tRightMotorIfPositiveTurn; + PWMDcMotor *tLeftMotorIfPositiveTurn; +#endif + if (aRotationDegrees >= 0) { + tRightMotorIfPositiveTurn = &rightCarMotor; + tLeftMotorIfPositiveTurn = &leftCarMotor; + } else { +// swap turn sign and left / right motors + aRotationDegrees = -aRotationDegrees; + tRightMotorIfPositiveTurn = &leftCarMotor; + tLeftMotorIfPositiveTurn = &rightCarMotor; + } + + /* + * Here aRotationDegrees is positive + * Now handle different turn directions + */ +#ifdef USE_MPU6050_IMU + unsigned int tDistanceMillimeter = 2000; // Dummy value for distance - equivalent to #define tDistanceCount 200 give a timeout of around 10 wheel rotations +#else + unsigned int tDistanceMillimeter = (aRotationDegrees * FactorDegreeToMillimeter) + 0.5; +#endif + + unsigned int tDistanceMillimeterRight; + unsigned int tDistanceMillimeterLeft; + + if (aTurnDirection == TURN_FORWARD) { + tDistanceMillimeterRight = tDistanceMillimeter; + tDistanceMillimeterLeft = 0; + } else if (aTurnDirection == TURN_BACKWARD) { + tDistanceMillimeterRight = 0; + tDistanceMillimeterLeft = tDistanceMillimeter; + } else { + tDistanceMillimeterRight = tDistanceMillimeter / 2; + tDistanceMillimeterLeft = tDistanceMillimeter / 2; + } + + /* + * Handle slow speed flag and reduce turn SpeedPWMs + */ + uint8_t tTurnSpeedPWMRight = tRightMotorIfPositiveTurn->DriveSpeedPWM; + uint8_t tTurnSpeedPWMLeft = tLeftMotorIfPositiveTurn->DriveSpeedPWM; + if (aUseSlowSpeed) { +// avoid overflow, the reduced SpeedPWM is almost max SpeedPWM then. + if (tRightMotorIfPositiveTurn->StartSpeedPWM < 160) { + tTurnSpeedPWMRight = tRightMotorIfPositiveTurn->StartSpeedPWM + (tRightMotorIfPositiveTurn->StartSpeedPWM / 2); + } + if (tLeftMotorIfPositiveTurn->StartSpeedPWM < 160) { + tTurnSpeedPWMLeft = tLeftMotorIfPositiveTurn->StartSpeedPWM + (tLeftMotorIfPositiveTurn->StartSpeedPWM / 2); + } + } + +#ifdef DEBUG + Serial.print(F("TurnSpeedPWMRight=")); + Serial.print(tTurnSpeedPWMRight); +# ifndef USE_MPU6050_IMU + Serial.print(F(" DistanceMillimeterRight=")); + Serial.print(tDistanceMillimeterRight); +# endif + Serial.println(); +#endif + +#ifdef USE_MPU6050_IMU + // We do not really have ramps for turn speed + if (tDistanceMillimeterRight > 0) { + tRightMotorIfPositiveTurn->setSpeedPWMCompensated(tTurnSpeedPWMRight, DIRECTION_FORWARD); + } + if (tDistanceMillimeterLeft > 0) { + tLeftMotorIfPositiveTurn->setSpeedPWMCompensated(tTurnSpeedPWMLeft, DIRECTION_BACKWARD); + } +#else + tRightMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMRight, tDistanceMillimeterRight, DIRECTION_FORWARD); + tLeftMotorIfPositiveTurn->startGoDistanceMillimeter(tTurnSpeedPWMLeft, tDistanceMillimeterLeft, DIRECTION_BACKWARD); +#endif +} + +/** + * @param aRotationDegrees positive -> turn left (counterclockwise), negative -> turn right + * @param aTurnDirection direction of turn TURN_FORWARD, TURN_BACKWARD or TURN_IN_PLACE (default) + * @param aUseSlowSpeed true (default) -> use slower SpeedPWM (1.5 times StartSpeedPWM) instead of DriveSpeedPWM for rotation to be more exact. + * Does not really work for 4WD cars. + * TODO remove? since only sensible for encoder motors. + * @param aLoopCallback avoid blocking and call aLoopCallback on waiting for stop + */ +void CarPWMMotorControl::rotate(int aRotationDegrees, uint8_t aTurnDirection, void (*aLoopCallback)(void), bool aUseSlowSpeed) { + if (aRotationDegrees != 0) { + startRotate(aRotationDegrees, aTurnDirection, aUseSlowSpeed); + waitUntilStopped(aLoopCallback); + } +} + +#ifdef USE_ENCODER_MOTOR_CONTROL +/* + * Get count / distance value from right motor + */ +unsigned int CarPWMMotorControl::getDistanceCount() { + return (rightCarMotor.EncoderCount); +} + +unsigned int CarPWMMotorControl::getDistanceMillimeter() { + return (rightCarMotor.getDistanceMillimeter()); +} + +#else +void CarPWMMotorControl::setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond) { + rightCarMotor.setMillimeterPerSecondForFixedDistanceDriving(aMillimeterPerSecond); + leftCarMotor.setMillimeterPerSecondForFixedDistanceDriving(aMillimeterPerSecond); +} + +#endif // USE_ENCODER_MOTOR_CONTROL + +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +unsigned int CarPWMMotorControl::getBrakingDistanceMillimeter() { +# ifdef USE_ENCODER_MOTOR_CONTROL + return rightCarMotor.getBrakingDistanceMillimeter(); +# else + unsigned int tCarSpeedCmPerSecond = CarSpeedCmPerSecondFromIMU; +// return (tCarSpeedCmPerSecond * tCarSpeedCmPerSecond * 100) / RAMP_DECELERATION_TIMES_2; // overflow! + return (tCarSpeedCmPerSecond * tCarSpeedCmPerSecond) / (RAMP_DECELERATION_TIMES_2 / 100); +# endif +} + +# ifdef USE_MPU6050_IMU +uint8_t CarPWMMotorControl::getTurnDistanceHalfDegree() { + uint8_t tCarTurn2DegreesPerSecondFromIMU = CarTurn2DegreesPerSecondFromIMU; + return ((tCarTurn2DegreesPerSecondFromIMU * tCarTurn2DegreesPerSecondFromIMU) / 20); +} +# endif + +/* + * Generates a rising ramp and detects the first movement -> this sets dead band / minimum SpeedPWM + * aLoopCallback is responsible for calling readCarDataFromMPU6050Fifo(); + */ +void CarPWMMotorControl::calibrate(void (*aLoopCallback)(void)) { + stop(); + resetControlValues(); + + rightCarMotor.StartSpeedPWM = 0; + leftCarMotor.StartSpeedPWM = 0; + +# ifdef USE_ENCODER_MOTOR_CONTROL + uint8_t tMotorMovingCount = 0; +# else + IMUData.resetOffsetDataAndWait(); +# endif + + /* + * increase motor SpeedPWM by 1 every 200 ms until motor moves + */ + for (uint8_t tSpeedPWM = 20; tSpeedPWM != MAX_SPEED_PWM; ++tSpeedPWM) { +// as long as no start speed is computed increase speed + if (rightCarMotor.StartSpeedPWM == 0) { + // as long as no start speed is computed, increase motor speed + rightCarMotor.setSpeedPWM(tSpeedPWM, DIRECTION_FORWARD); + } + if (leftCarMotor.StartSpeedPWM == 0) { + leftCarMotor.setSpeedPWM(tSpeedPWM, DIRECTION_FORWARD); + } + + /* + * Active delay of 200 ms + */ + uint32_t tStartMillis = millis(); + do { + if (aLoopCallback != NULL) { + aLoopCallback(); + } + if (isStopped()) { + // we were stopped by aLoopCallback() + return; + } +# ifdef USE_ENCODER_MOTOR_CONTROL + delay(10); +# else + delay(DELAY_TO_NEXT_IMU_DATA_MILLIS); + updateIMUData(); +# endif + } while (millis() - tStartMillis <= 200); + + /* + * Check if wheel moved + */ +# ifdef USE_ENCODER_MOTOR_CONTROL + /* + * Store speed after 6 counts (3cm) + */ + if (rightCarMotor.StartSpeedPWM == 0 && rightCarMotor.EncoderCount > 6) { + rightCarMotor.setStartSpeedPWM(tSpeedPWM); + tMotorMovingCount++; + } + if (leftCarMotor.StartSpeedPWM == 0 && leftCarMotor.EncoderCount > 6) { + leftCarMotor.setStartSpeedPWM(tSpeedPWM); + tMotorMovingCount++; + } + if (tMotorMovingCount >= 2) { + // Do not end loop if one motor is still not moving + break; + } + +# else + if (abs(IMUData.getSpeedCmPerSecond()) >= 10) { + setStartSpeedPWM(tSpeedPWM); + break; + } +# endif + } + stop(); +} +#endif // defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) diff --git a/src/CarPWMMotorControl.h b/src/CarPWMMotorControl.h new file mode 100644 index 0000000..d18eb71 --- /dev/null +++ b/src/CarPWMMotorControl.h @@ -0,0 +1,206 @@ +/* + * CarPWMMotorControl.h + * + * Motor control for a car with 2 encoder motors + * + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef CarPWMMotorControl_H_ +#define CarPWMMotorControl_H_ + +#include "EncoderMotor.h" + +/* + * Use GY-521 MPU6050 breakout board connected by I2C for support of precise turning and speed / distance calibration. + * Connectors point to the rear. + */ +//#define USE_MPU6050_IMU +#ifdef USE_MPU6050_IMU +#include "IMUCarData.h" +#endif +#include + +/* + * Values for 20 slot encoder discs. Circumference of the wheel is 22.0 cm + * Distance between two wheels is around 14 cm -> 360 degree are 82 cm + */ +#define FACTOR_DEGREE_TO_MILLIMETER_2WD_CAR_DEFAULT 2.2777 +#define FACTOR_DEGREE_TO_MILLIMETER_4WD_CAR_DEFAULT 5.0 // estimated, with slip + +#if ! defined(FACTOR_DEGREE_TO_MILLIMETER_DEFAULT) +# if defined(CAR_HAS_4_WHEELS) +#define FACTOR_DEGREE_TO_MILLIMETER_DEFAULT FACTOR_DEGREE_TO_MILLIMETER_4WD_CAR_DEFAULT +# else +#define FACTOR_DEGREE_TO_MILLIMETER_DEFAULT FACTOR_DEGREE_TO_MILLIMETER_2WD_CAR_DEFAULT +# endif +#endif + +// turn directions +#define TURN_FORWARD DIRECTION_FORWARD // 0 +#define TURN_BACKWARD DIRECTION_BACKWARD // 1 +#define TURN_IN_PLACE 2 + +class CarPWMMotorControl { +public: + + CarPWMMotorControl(); +// virtual ~CarPWMMotorControl(); + +#ifdef USE_ADAFRUIT_MOTOR_SHIELD + void init(); +#else + void init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, uint8_t aLeftMotorForwardPin, + uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin); + void init(uint8_t aRightMotorForwardPin, uint8_t aRightMotorBackwardPin, uint8_t aRightPWMPin, uint8_t aRightInterruptNumber, + uint8_t aLeftMotorForwardPin, uint8_t LeftMotorBackwardPin, uint8_t aLeftMotorPWMPin, uint8_t aLeftInterruptNumber); +#endif // USE_ADAFRUIT_MOTOR_SHIELD + + void setDefaultsForFixedDistanceDriving(); + void setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, int8_t aSpeedPWMCompensationRight); + void changeSpeedPWMCompensation(int8_t aSpeedPWMCompensationRight); + void setStartSpeedPWM(uint8_t aStartSpeedPWM); + void setDriveSpeedPWM(uint8_t aDriveSpeedPWM); + + void writeMotorValuesToEeprom(); + void readMotorValuesFromEeprom(); + +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + void calibrate(void (*aLoopCallback)(void)); // aLoopCallback must call readCarDataFromMPU6050Fifo() + unsigned int getBrakingDistanceMillimeter(); + uint8_t getTurnDistanceHalfDegree(); +#endif + +#ifdef USE_MPU6050_IMU + void updateIMUData(); + void calculateAndPrintIMUOffsets(Print *aSerial); +#endif + +#ifdef USE_ENCODER_MOTOR_CONTROL + // retrieves values from right motor + unsigned int getDistanceCount(); + unsigned int getDistanceMillimeter(); +#else + // makes no sense for encoder motor + void setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond); +#endif + + // If ramp up is not supported, these functions just sets the speed and return immediately + void startRampUp(uint8_t aRequestedDirection = DIRECTION_FORWARD); + void startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void waitForDriveSpeedPWM(void (*aLoopCallback)(void) = NULL); + void startRampUpAndWait(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection = DIRECTION_FORWARD, + void (*aLoopCallback)(void) = NULL); + void startRampUpAndWaitForDriveSpeedPWM(uint8_t aRequestedDirection = DIRECTION_FORWARD, void (*aLoopCallback)(void) = NULL); + + /* + * For car direction handling + */ + uint8_t getCarDirectionOrBrakeMode(); + uint8_t CarDirectionOrBrakeMode; + + /* + * Functions for moving a fixed distance + */ + // With signed distance + void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values + void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); // only setup values + void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // only setup values, no movement -> use updateMotors() + + void goDistanceMillimeter(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped + void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection, + void (*aLoopCallback)(void) = NULL); // Blocking function, uses waitUntilStopped + + bool checkAndHandleDirectionChange(uint8_t aRequestedDirection); // used internally + + /* + * Functions for rotation + */ + void setFactorDegreeToMillimeter(float aFactorDegreeToMillimeter); +#if defined(CAR_HAS_4_WHEELS) + // slow speed does not really work for 4 WD cars + void startRotate(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = false); + void rotate(int aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, + void (*aLoopCallback)(void) = NULL, bool aUseSlowSpeed = false); +#else + void startRotate(int aRotationDegrees, uint8_t aTurnDirection, bool aUseSlowSpeed = true); + void rotate(int aRotationDegrees, uint8_t aTurnDirection = TURN_IN_PLACE, + void (*aLoopCallback)(void) = NULL, bool aUseSlowSpeed = true); +#endif + +#ifdef USE_MPU6050_IMU + IMUCarData IMUData; + int CarRequestedRotationDegrees; // 0 -> car is moving forward / backward + int CarTurnAngleHalfDegreesFromIMU; // Read from Gyroscope + int8_t CarTurn2DegreesPerSecondFromIMU; // Read from Gyroscope + // Read from Accelerator + unsigned int CarSpeedCmPerSecondFromIMU; + unsigned int CarRequestedDistanceMillimeter; + unsigned int CarDistanceMillimeterFromIMU; +#else + float FactorDegreeToMillimeter; +#endif + + bool updateMotors(); + bool updateMotors(void (*aLoopCallback)(void)); + void delayAndUpdateMotors(unsigned int aDelayMillis); + + /* + * Start/Stop functions + */ + void startRampDown(); + void stopAndWaitForIt(void (*aLoopCallback)(void) = NULL); // uses waitUntilStopped() + void waitUntilStopped(void (*aLoopCallback)(void) = NULL); + + /* + * Check motor state functions + */ + bool isStopped(); + bool isState(uint8_t aState); + bool isStateRamp(); // MOTOR_STATE_RAMP_UP OR MOTOR_STATE_RAMP_DOWN + + void resetControlValues(); + + /* + * Functions, which directly call motor functions for both motors + */ + void changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM); // Keeps direction + void setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection, int8_t aLeftRightSpeedPWM); + void stop(uint8_t aStopMode = STOP_MODE_KEEP); // STOP_MODE_KEEP (take previously defined DefaultStopMode) or MOTOR_BRAKE or MOTOR_RELEASE + + void setSpeedPWM(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void setStopMode(uint8_t aStopMode); + void setSpeedPWM(int aRequestedSpeedPWM); + void setSpeedPWMCompensated(int aRequestedSpeedPWM); + +#ifdef USE_ENCODER_MOTOR_CONTROL + EncoderMotor rightCarMotor; // 40 bytes RAM + EncoderMotor leftCarMotor; +#else + PWMDcMotor rightCarMotor; + PWMDcMotor leftCarMotor; +#endif +}; + +#endif /* CarPWMMotorControl_H_ */ + +#pragma once + diff --git a/src/DebugLevel.h b/src/DebugLevel.h new file mode 100644 index 0000000..cbb2d94 --- /dev/null +++ b/src/DebugLevel.h @@ -0,0 +1,52 @@ +/* + * DebugLevel.h + * Include to propagate debug levels + * + * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Email: armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-Utils https://github.com/ArminJo/Arduino-Utils. + * + * Arduino-Utils is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef DEBUGLEVEL_H_ +#define DEBUGLEVEL_H_ + +// Propagate debug level +#ifdef TRACE // Information you need to understand details of a function or if you hunt a bug. +# ifndef DEBUG +#define DEBUG // Information need to understand the operating of your program. E.g. function calls and values of control variables. +# endif +#endif +#ifdef DEBUG +# ifndef INFO +#define INFO // Information you want to see in regular operation to see what the program is doing. E.g. "START ../src/LightToTone.cpp Version 1.2 from Dec 31 2019" or "Now playing Muppets melody". +# endif +#endif +#ifdef INFO +# ifndef WARN +#define WARN // Information that the program may encounter problems, like small Heap/Stack area. +# endif +#endif +#ifdef WARN +# ifndef ERROR +#define ERROR // Informations to explain why the program will not run. E.g. not enough Ram for all created objects. +# endif +#endif + +#endif /* DEBUGLEVEL_H_ */ + +#pragma once diff --git a/src/Distance.cpp b/src/Distance.cpp index 7a036e4..8033d31 100644 --- a/src/Distance.cpp +++ b/src/Distance.cpp @@ -25,7 +25,9 @@ ForwardDistancesInfoStruct sForwardDistancesInfo; +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) Servo DistanceServo; +#endif uint8_t sLastServoAngleInDegrees; // 0 - 180 needed for optimized delay for servo repositioning. Only set by DistanceServoWriteAndDelay() #ifdef CAR_HAS_TOF_DISTANCE_SENSOR @@ -118,7 +120,11 @@ void DistanceServoWriteAndDelay(uint8_t aTargetDegrees, bool doDelay) { // The servo is top down and therefore inverted aTargetDegrees = 180 - aTargetDegrees; #endif +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) DistanceServo.write(aTargetDegrees); +#else + write10(aTargetDegrees); +#endif /* * Delay @@ -179,7 +185,7 @@ int scanForTarget() { */ for (uint8_t i = 0; i < 3; ++i) { DistanceServoWriteAndDelay(tScanDegree, true); - tCentimeter = getDistanceAsCentiMeter(false, DISTANCE_TIMEOUT_CM_FOLLOWER); + tCentimeter = getDistanceAsCentiMeter(false, DISTANCE_TIMEOUT_CM_FOLLOWER, true); uint8_t tCurrentIndex; if (tDeltaDegree > 0) { @@ -194,17 +200,17 @@ int scanForTarget() { * Determine color and draw distance line */ color16_t tColor; - tColor = COLOR_RED; // tCentimeter <= sCentimeterPerScan + tColor = COLOR16_RED; // tCentimeter <= sCentimeterPerScan if (tCentimeter <= FOLLOWER_DISTANCE_TARGET_SCAN_CENTIMETER) { - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } else if (tCentimeter < FOLLOWER_DISTANCE_MINIMUM_CENTIMETER) { - tColor = COLOR_YELLOW; + tColor = COLOR16_YELLOW; } // Clear old line BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, sForwardDistancesInfo.RawDistancesArray[tCurrentIndex], tScanDegree, - COLOR_WHITE, 3); + COLOR16_WHITE, 3); BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tScanDegree, tColor, 3); } sForwardDistancesInfo.RawDistancesArray[tCurrentIndex] = tCentimeter; @@ -223,7 +229,7 @@ int scanForTarget() { */ sprintf_P(sStringBuffer, PSTR("rotation:%3d\xB0 min:%2dcm"), tScanDegree - 90, tCentimeter); // \xB0 is degree character BlueDisplay1.drawText(BUTTON_WIDTH_3_5_POS_2, US_DISTANCE_MAP_ORIGIN_Y + TEXT_SIZE_11, sStringBuffer, TEXT_SIZE_11, - COLOR_BLACK, COLOR_WHITE); + COLOR16_BLACK, COLOR16_WHITE); // reset distance servo direction DistanceServoWriteAndDelay(90, false); @@ -282,33 +288,32 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b // User sent an event -> stop and return now return true; } - - unsigned int tCentimeter = getDistanceAsCentiMeter(false); + unsigned int tCentimeter = getDistanceAsCentiMeter(false, DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, true); if ((tIndex == INDEX_FORWARD_1 || tIndex == INDEX_FORWARD_2) && tCentimeter <= sCentimeterPerScanTimesTwo) { /* * Emergency motor stop if index is forward and measured distance is less than distance driven during two scans */ - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); } if (sCurrentPage == PAGE_AUTOMATIC_CONTROL && BlueDisplay1.isConnectionEstablished()) { /* * Determine color */ - tColor = COLOR_RED; // tCentimeter <= sCentimeterPerScan + tColor = COLOR16_RED; // tCentimeter <= sCentimeterPerScan if (tCentimeter >= DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE) { tColor = DISTANCE_TIMEOUT_COLOR; } else if (tCentimeter > sCentimeterPerScanTimesTwo) { - tColor = COLOR_GREEN; + tColor = COLOR16_GREEN; } else if (tCentimeter > sCentimeterPerScan) { - tColor = COLOR_YELLOW; + tColor = COLOR16_YELLOW; } /* * Clear old and draw new distance line */ BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, - sForwardDistancesInfo.RawDistancesArray[tIndex], tCurrentDegrees, COLOR_WHITE, 3); + sForwardDistancesInfo.RawDistancesArray[tIndex], tCurrentDegrees, COLOR16_WHITE, 3); BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tCentimeter, tCurrentDegrees, tColor, 3); } @@ -337,7 +342,7 @@ bool __attribute__((weak)) fillAndShowForwardDistancesInfo(bool aDoFirstValue, b * Negative means we are heading away from wall */ uint8_t computeNeigbourValue(uint8_t aDegreesPerStepValue, uint8_t a2DegreesPerStepValue, uint8_t aClipValue, - int8_t * aDegreeOfEndpointConnectingLine) { + int8_t *aDegreeOfEndpointConnectingLine) { /* * Name of the variables are for DEGREES_PER_STEP = 20 only for better understanding. @@ -443,7 +448,7 @@ void doWallDetection() { * i.e. put 20 degrees to 40 degrees parameter and vice versa in order to use the 0 degree value as the 60 degrees one */ uint8_t tNextDistanceComputed = computeNeigbourValue(tCurrentDistance, tLastDistance, - DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); + DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); #ifdef TRACE BlueDisplay1.debug("i=", i); BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); @@ -482,7 +487,7 @@ void doWallDetection() { tNextDistanceOriginal = tNextDistanceComputed; if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextDistanceComputed, - tCurrentAngleToCheck, COLOR_WHITE, 1); + tCurrentAngleToCheck, COLOR16_WHITE, 1); } } } @@ -520,7 +525,7 @@ void doWallDetection() { * Use computeNeigbourValue in the intended way, so do not change sign of tDegreeOfConnectingLine! */ uint8_t tNextValueComputed = computeNeigbourValue(tCurrentDistance, tLastDistance, - DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); + DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, &tDegreeOfConnectingLine); #ifdef TRACE BlueDisplay1.debug("i=", i); BlueDisplay1.debug("AngleToCheck @i+1=", tCurrentAngleToCheck); @@ -556,7 +561,7 @@ void doWallDetection() { tNextValue = tNextValueComputed; if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { BlueDisplay1.drawVectorDegrees(US_DISTANCE_MAP_ORIGIN_X, US_DISTANCE_MAP_ORIGIN_Y, tNextValueComputed, - tCurrentAngleToCheck, COLOR_WHITE, 1); + tCurrentAngleToCheck, COLOR16_WHITE, 1); } } } @@ -605,23 +610,27 @@ void postProcessDistances(uint8_t aDistanceThreshold) { } } -void readAndShowDistancePeriodically(uint16_t aPeriodMillis) { +void readAndShowDistancePeriodically() { static long sLastUSMeasurementMillis; // Do not show distanced during (time critical) acceleration or deceleration if (!RobotCarMotorControl.isStateRamp()) { long tMillis = millis(); - if (sLastUSMeasurementMillis + aPeriodMillis < tMillis) { + if (tMillis - sLastUSMeasurementMillis >= DISTANCE_DISPLAY_PERIOD_MILLIS) { sLastUSMeasurementMillis = tMillis; - getDistanceAsCentiMeter(true); + getDistanceAsCentiMeter(true, DISTANCE_TIMEOUT_CM, false); } } } /* * Timeout is DISTANCE_TIMEOUT_CM (1 meter) + * aWaitForCurrentMeasurmentToEnd for IR Distance sensors if true, wait for the current measurement to end, since the sensor was recently moved. */ -unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { +unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout, bool aWaitForCurrentMeasurementToEnd) { +#if !defined(CAR_HAS_IR_DISTANCE_SENSOR) + (void) aWaitForCurrentMeasurementToEnd; // not used +#endif #ifdef CAR_HAS_TOF_DISTANCE_SENSOR if (sScanMode != SCAN_MODE_US) { sToFDistanceSensor.VL53L1X_StartRanging(); @@ -629,6 +638,9 @@ unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { #endif unsigned int tCentimeter = getUSDistanceAsCentiMeterWithCentimeterTimeout(aDistanceTimeout); + if (tCentimeter == 0) { + tCentimeter = aDistanceTimeout; + } if (doShow) { showUSDistance(tCentimeter); } @@ -637,7 +649,7 @@ unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { if (sScanMode != SCAN_MODE_US) { # if defined(CAR_HAS_IR_DISTANCE_SENSOR) if (sScanMode != SCAN_MODE_US) { - tIRCentimeter = getIRDistanceAsCentimeter(); + tIRCentimeter = getIRDistanceAsCentimeter(aWaitForCurrentMeasurementToEnd); if (doShow) { showIRDistance(tIRCentimeter); } @@ -672,8 +684,30 @@ unsigned int getDistanceAsCentiMeter(bool doShow, uint8_t aDistanceTimeout) { /* * The 1080 needs 39 ms for each measurement cycle */ -uint8_t getIRDistanceAsCentimeter() { - float tVolt = analogRead(PIN_IR_DISTANCE_SENSOR); +uint8_t getIRDistanceAsCentimeter(bool aWaitForCurrentMeasurementToEnd) { + uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(PIN_IR_DISTANCE_SENSOR, DEFAULT); + // check for voltage changed then a new measurement is started + if (aWaitForCurrentMeasurementToEnd) { + uint16_t tOldValue = readADCChannelWithReferenceOversampleFast(PIN_IR_DISTANCE_SENSOR, DEFAULT, 2); // 4 samples +// int16_t tOldValue = analogRead(PIN_IR_DISTANCE_SENSOR); // 100 us + uint32_t tStartMillis = millis(); + do { + int16_t tNewValue = readADCChannelWithReferenceOversampleFast(PIN_IR_DISTANCE_SENSOR, DEFAULT, 2); // 100 us +// int16_t tNewValue = analogRead(PIN_IR_DISTANCE_SENSOR); // 100 us + if (abs(tOldValue-tNewValue) > IR_SENSOR_NEW_MEASUREMENT_THRESHOLD) { + // assume, that voltage has changed because of the end of a measurement + break; + } + loopGUI(); + } while (millis() - tStartMillis <= IR_SENSOR_MEASUREMENT_TIME_MILLIS); + // now a new measurement has started, wait for the result + delayAndLoopGUI(IR_SENSOR_NEW_MEASUREMENT_THRESHOLD); // the IR sensor takes 39 ms for one measurement + } + + float tVolt = readADCChannelWithReferenceOversampleFast(PIN_IR_DISTANCE_SENSOR, DEFAULT, 2); // 4 samples +// float tVolt = analogRead(PIN_IR_DISTANCE_SENSOR); // 100 us + ADMUX = tOldADMUX; // Switch back (to INTERNAL) + // * 0.004887585 for 1023 = 5V // Model 1080 / GP2Y0A21YK0F return (29.988 * pow(tVolt * 0.004887585, -1.173)) + 0.5; // see https://github.com/guillaume-rico/SharpIR/blob/master/SharpIR.cpp diff --git a/src/Distance.h b/src/Distance.h index e9f1202..7cfa3b5 100644 --- a/src/Distance.h +++ b/src/Distance.h @@ -20,13 +20,20 @@ #define DISTANCE_H_ #include + +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) #include +#else +#include "LightweightServo.h" +#endif /* - * Comment this out / enable this if the distance servo is mounted head down to detect small obstacles. + * Activate this, if the distance servo is mounted head down to detect small obstacles. */ //#define DISTANCE_SERVO_IS_MOUNTED_HEAD_DOWN +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) extern Servo DistanceServo; +#endif /* * Constants for fillAndShowForwardDistancesInfo(), doWallDetection etc. @@ -36,10 +43,11 @@ extern Servo DistanceServo; #define STEPS_PER_SCAN (NUMBER_OF_DISTANCES - 1) // -> 162 degrees for 18 DEGREES_PER_STEP, 153 for 17 degrees #define START_DEGREES ((180 - (DEGREES_PER_STEP * STEPS_PER_SCAN)) / 2) // 9 for 18, 13,5 for 17 - we need it symmetrical in the 180 degrees range +#define DISTANCE_TIMEOUT_CM 200 // do not measure distances greater than 200 cm #define DISTANCE_TIMEOUT_CM_FOLLOWER 130 // do not measure and process distances greater than 130 cm #define DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE 100 // do not measure and process distances greater than 100 cm -#define DISTANCE_TIMEOUT_COLOR COLOR_CYAN +#define DISTANCE_TIMEOUT_COLOR COLOR16_CYAN #define DISTANCE_DISPLAY_PERIOD_MILLIS 500 // for future use maybe @@ -83,7 +91,8 @@ extern int sLastDegreesTurned; void initDistance(); void DistanceServoWriteAndDelay(uint8_t aValue, bool doDelay = false); -unsigned int getDistanceAsCentiMeter(bool doShow = false, uint8_t aDistanceTimeout = DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE); +unsigned int getDistanceAsCentiMeter(bool doShow = false, uint8_t aDistanceTimeout = DISTANCE_TIMEOUT_CM_AUTONOMOUS_DRIVE, + bool aWaitForCurrentMeasurementToEnd = false); int scanForTarget(); bool fillAndShowForwardDistancesInfo(bool aDoFirstValue, bool aForceScan = false); void doWallDetection(); @@ -99,7 +108,9 @@ uint8_t readToFDistanceAsCentimeter(); // no start of measurement, just read res #endif #ifdef CAR_HAS_IR_DISTANCE_SENSOR -uint8_t getIRDistanceAsCentimeter(); +uint8_t getIRDistanceAsCentimeter(bool aWaitForCurrentMeasurementToEnd); +#define IR_SENSOR_NEW_MEASUREMENT_THRESHOLD 2 // If the output value changes by this amount, we can assume that a new measurement is started +#define IR_SENSOR_MEASUREMENT_TIME_MILLIS 41 // the IR sensor takes 39 ms for one measurement #endif #endif // DISTANCE_H_ diff --git a/src/EncoderMotor.cpp b/src/EncoderMotor.cpp index 98e2bff..e41e5e4 100644 --- a/src/EncoderMotor.cpp +++ b/src/EncoderMotor.cpp @@ -11,12 +11,17 @@ * * Tested for Adafruit Motor Shield and plain TB6612 breakout board. * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -27,9 +32,14 @@ */ #include +#if defined(USE_ENCODER_MOTOR_CONTROL) #include "EncoderMotor.h" -volatile bool EncoderMotor::EncoderTickCounterHasChanged; +//#define TRACE +//#define DEBUG + +EncoderMotor *sPointerForInt0ISR; +EncoderMotor *sPointerForInt1ISR; EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly initialized") PWMDcMotor() { @@ -37,7 +47,6 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in /* * The list version saves 100 bytes and is more flexible, compared with the array version */ - MotorValuesEepromStorageNumber = EncoderMotor::sNumberOfMotorControls; EncoderMotor::sNumberOfMotorControls++; NextMotorControl = NULL; if (sMotorControlListStart == NULL) { @@ -45,7 +54,7 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in sMotorControlListStart = this; } else { // put object in control list - EncoderMotor * tObjectPointer = sMotorControlListStart; + EncoderMotor *tObjectPointer = sMotorControlListStart; // search last list element while (tObjectPointer->NextMotorControl != NULL) { tObjectPointer = tObjectPointer->NextMotorControl; @@ -57,70 +66,71 @@ EncoderMotor::EncoderMotor() : // @suppress("Class members should be properly in } #ifdef USE_ADAFRUIT_MOTOR_SHIELD -void EncoderMotor::init(uint8_t aMotorNumber, bool aReadFromEeprom) { - PWMDcMotor::init(aMotorNumber, aReadFromEeprom); // create with the default frequency 1.6KHz - resetControlValues(); +/* + * aMotorNumber from 1 to 2 + * If no parameter, we use a fixed assignment of rightCarMotor interrupts to INT0 / Pin2 and leftCarMotor to INT1 / Pin3 + * Currently motors 3 and 4 are not required/supported by own library for Adafruit Motor Shield + */ +void EncoderMotor::init(uint8_t aMotorNumber) { + PWMDcMotor::init(aMotorNumber); // create with the default frequency 1.6KHz + resetEncoderControlValues(); +} +void EncoderMotor::init(uint8_t aMotorNumber, uint8_t aInterruptNumber) { + PWMDcMotor::init(aMotorNumber); // create with the default frequency 1.6KHz + resetEncoderControlValues(); + attachInterrupt(aInterruptNumber); } #else -void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber) { - PWMDcMotor::init(aForwardPin, aBackwardPin, aPWMPin, aMotorNumber); - resetControlValues(); +void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin) { + PWMDcMotor::init(aForwardPin, aBackwardPin, aPWMPin); + resetEncoderControlValues(); +} + +void EncoderMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aInterruptNumber) { + PWMDcMotor::init(aForwardPin, aBackwardPin, aPWMPin); + resetEncoderControlValues(); + attachInterrupt(aInterruptNumber); } #endif /* - * If motor is already running, adjust TargetDistanceCount to go to aRequestedDistanceCount + * If motor is already running, adjust TargetDistanceCount to go to aRequestedDistanceMillimeter */ -void EncoderMotor::startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { -// if (aRequestedDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { -// PanicWithLed(400, 22); -// } - if (aRequestedDistanceCount == 0) { +void EncoderMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + if (aRequestedDistanceMillimeter == 0) { + stop(DefaultStopMode); // In case motor was running return; } - if (CurrentSpeed == 0) { - TargetDistanceCount = aRequestedDistanceCount; -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; // This in turn resets EncoderCount etc. at first call of updateMotor() - CurrentDriveSpeed = aRequestedSpeed; - setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode -#else - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); -#endif + if (CurrentSpeedPWM == 0) { + startRampUp(aRequestedSpeedPWM, aRequestedDirection); + TargetDistanceMillimeter = aRequestedDistanceMillimeter; } else { - TargetDistanceCount = EncoderCount + aRequestedDistanceCount; /* - * prolong NextChangeMaxTargetCount for the new distance + * Already moving */ -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_DRIVE_SPEED; - uint8_t tDistanceCountForRampDown = DistanceCountAfterRampUp; - // guarantee minimal ramp down length - if (tDistanceCountForRampDown < 3 && TargetDistanceCount > 6) { - tDistanceCountForRampDown = 3; - } - NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown; -#endif - PWMDcMotor::setSpeed(aRequestedSpeed, aRequestedDirection); + MotorRampState = MOTOR_STATE_DRIVE; // must be set, since we may be moving until now without ramp control + TargetDistanceMillimeter = getDistanceMillimeter() + aRequestedDistanceMillimeter; + PWMDcMotor::setSpeedPWM(aRequestedSpeedPWM, aRequestedDirection); } - LastTargetDistanceCount = TargetDistanceCount; - MotorMovesFixedDistance = true; + LastTargetDistanceMillimeter = TargetDistanceMillimeter; + CheckDistanceInUpdateMotor = true; } -void EncoderMotor::startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +void EncoderMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } /* - * if aRequestedDistanceCount < 0 then use DIRECTION_BACKWARD - * initialize motorInfo fields DirectionForward, CurrentDriveSpeed, DistanceTickCounter and optional NextChangeMaxTargetCount. + * if aRequestedDistanceMillimeter < 0 then use DIRECTION_BACKWARD + * initialize motorInfo fields DirectionForward, RequestedDriveSpeedPWM, DistanceTickCounter and optional NextChangeMaxTargetCount. */ -void EncoderMotor::startGoDistanceCount(int aRequestedDistanceCount) { - if (aRequestedDistanceCount < 0) { - aRequestedDistanceCount = -aRequestedDistanceCount; - startGoDistanceCount(aRequestedDistanceCount, DIRECTION_BACKWARD); +void EncoderMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceCount(aRequestedDistanceCount, DIRECTION_FORWARD); + startGoDistanceMillimeter(aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } @@ -129,164 +139,189 @@ void EncoderMotor::startGoDistanceCount(int aRequestedDistanceCount) { */ bool EncoderMotor::updateMotor() { unsigned long tMillis = millis(); -#ifdef SUPPORT_RAMP_UP - uint8_t tNewSpeed = CurrentSpeed; + uint8_t tNewSpeedPWM = CurrentSpeedPWM; if (MotorRampState == MOTOR_STATE_START) { - // --> RAMP_UP - MotorRampState = MOTOR_STATE_RAMP_UP; - LastRideEncoderCount = 0; - EncoderCount = 0; + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; /* * Start motor */ - tNewSpeed = StartSpeed; - NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS; - NextChangeMaxTargetCount = TargetDistanceCount / 2; - // initialize for timeout detection - EncoderTickLastMillis = tMillis - ENCODER_SENSOR_MASK_MILLIS - 1; - - RampDelta = RAMP_UP_VALUE_DELTA; // 16 steps a 16 millis for ramp up => 256 milliseconds - if (RampDelta < 2) { - RampDelta = 2; + if (RequestedDriveSpeedPWM > RAMP_VALUE_UP_OFFSET_SPEED_PWM) { + // start with ramp to avoid spinning wheels + tNewSpeedPWM = RAMP_VALUE_UP_OFFSET_SPEED_PWM; // start immediately with speed offset (3 volt) + // --> RAMP_UP + MotorRampState = MOTOR_STATE_RAMP_UP; + } else { + // Motor ramp not required, go direct to drive speed. + tNewSpeedPWM = RequestedDriveSpeedPWM; + // --> DRIVE + MotorRampState = MOTOR_STATE_DRIVE; } - DebugCount = 0; - Debug = 0; - } - // do not use else if since state can be changed in code before - if (MotorRampState == MOTOR_STATE_RAMP_UP) { /* - * Increase motor speed RAMP_UP_UPDATE_INTERVAL_STEPS (16) times every RAMP_UP_UPDATE_INTERVAL_MILLIS (16) milliseconds - * or until more than half of distance is done - * Distance required for ramp is 0 to 10 or more, increasing with increasing CurrentDriveSpeed + * Init Encoder values */ - if (tMillis >= NextRampChangeMillis) { - NextRampChangeMillis += RAMP_UP_UPDATE_INTERVAL_MILLIS; - tNewSpeed = tNewSpeed + RampDelta; - // Clip value and check for 8 bit overflow - if (tNewSpeed > CurrentDriveSpeed || tNewSpeed <= RampDelta) { - tNewSpeed = CurrentDriveSpeed; - } + LastRideEncoderCount = 0; + EncoderCount = 0; + // initialize for timeout detection + LastEncoderInterruptMillis = tMillis - ENCODER_SENSOR_RING_MILLIS - 1; + } else if (MotorRampState == MOTOR_STATE_RAMP_UP) { + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis += RAMP_INTERVAL_MILLIS; /* - * Transition criteria is: - * Max Speed reached or more than half of distance is done + * Increase motor speed by RAMP_UP_VALUE_DELTA every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + * Transition criteria to next state is: + * Drive speed reached or target distance - braking distance reached */ - if (tNewSpeed == CurrentDriveSpeed || (MotorMovesFixedDistance && EncoderCount >= NextChangeMaxTargetCount)) { - // --> DRIVE_SPEED - MotorRampState = MOTOR_STATE_DRIVE_SPEED; - - DistanceCountAfterRampUp = EncoderCount; - uint8_t tDistanceCountForRampDown = DistanceCountAfterRampUp; - // guarantee minimal ramp down length - if (tDistanceCountForRampDown < 3 && TargetDistanceCount > 6) { - tDistanceCountForRampDown = 3; + if (tNewSpeedPWM == RequestedDriveSpeedPWM + || (CheckDistanceInUpdateMotor + && getDistanceMillimeter() + getBrakingDistanceMillimeter() >= TargetDistanceMillimeter)) { + // RequestedDriveSpeedPWM reached switch to --> DRIVE_SPEED_PWM and check immediately for next transition to RAMP_DOWN + MotorRampState = MOTOR_STATE_DRIVE; + } else { + tNewSpeedPWM = tNewSpeedPWM + RampSpeedPWMDelta; + // Clip value and check for 8 bit overflow + if (tNewSpeedPWM > RequestedDriveSpeedPWM || tNewSpeedPWM <= RampSpeedPWMDelta) { + // do not change state here to let motor run at RequestedDriveSpeedPWM for one interval + tNewSpeedPWM = RequestedDriveSpeedPWM; } - NextChangeMaxTargetCount = TargetDistanceCount - tDistanceCountForRampDown; } +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" St=")); + Serial.print(MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(tNewSpeedPWM); +#endif } } - // do not use else if since state can be changed in code before - if (MotorRampState == MOTOR_STATE_DRIVE_SPEED) { + // do not use "else if" since we must immediately check for next transition to RAMP_DOWN + if (MotorRampState == MOTOR_STATE_DRIVE) { /* - * Wait until ramp down count is reached + * Wait until target distance - braking distance reached */ - if (MotorMovesFixedDistance && EncoderCount >= NextChangeMaxTargetCount) { + if (CheckDistanceInUpdateMotor && getDistanceMillimeter() + getBrakingDistanceMillimeter() >= TargetDistanceMillimeter) { + if (CurrentSpeedPWM > (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA)) { + tNewSpeedPWM -= (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA); // RAMP_DOWN_VALUE_DELTA is immediately subtracted below + } else { + tNewSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; + } // --> RAMP_DOWN MotorRampState = MOTOR_STATE_RAMP_DOWN; - /* - * Ramp to reach StartSpeed after 1/2 of remaining distance - * TODO reduce speed just here? e.g. to 1.5 of start speed? - */ - RampDeltaPerDistanceCount = ((CurrentSpeed - StartSpeed) * 2) / ((TargetDistanceCount - EncoderCount)) + 1; +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Dist=")); + Serial.print(getDistanceMillimeter()); + Serial.print(F(" Breakdist=")); + Serial.print(getBrakingDistanceMillimeter()); + Serial.print(F(" St=")); + Serial.print(MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(tNewSpeedPWM); +#endif } } - // do not use else if since state can be changed in code before + // do not use "else if" since we must immediately check for next transition to STOPPED if (MotorRampState == MOTOR_STATE_RAMP_DOWN) { - DebugCount++; - - /* - * Decrease motor speed depending on distance to target count - */ - if (EncoderCount >= NextChangeMaxTargetCount) { - Debug++; - NextChangeMaxTargetCount++; - if (tNewSpeed > RampDeltaPerDistanceCount) { - tNewSpeed -= RampDeltaPerDistanceCount; + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; + /* + * Decrease motor speed RAMP_UP_UPDATE_INTERVAL_STEPS times every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + */ + if (tNewSpeedPWM > (RAMP_DOWN_VALUE_DELTA + RAMP_VALUE_DOWN_MIN_SPEED_PWM)) { + tNewSpeedPWM -= RAMP_DOWN_VALUE_DELTA; } else { - tNewSpeed = StartSpeed; - } - // safety net for slow speed - if (tNewSpeed < StartSpeed) { - tNewSpeed = StartSpeed; + tNewSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; } +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" St=")); + Serial.print(MotorRampState); + Serial.print(F(" Ns=")); + Serial.println(tNewSpeedPWM); +#endif } + } // End of motor state machine - if (tNewSpeed != CurrentSpeed) { - PWMDcMotor::setSpeed(tNewSpeed, CurrentDirectionOrBrakeMode); - } +#ifdef TRACE + Serial.print(PWMPin); + Serial.print(F(" St=")); + Serial.println(MotorRampState); +#endif + if (tNewSpeedPWM != CurrentSpeedPWM) { +#ifdef TRACE + Serial.print(F("Ns=")); + Serial.println(tNewSpeedPWM); #endif + PWMDcMotor::setSpeedPWM(tNewSpeedPWM, CurrentDirectionOrBrakeMode); + } /* - * Check if target count is reached or encoder tick timeout + * Check if target distance is reached or encoder tick timeout */ - if (CurrentSpeed > 0) { - if (MotorMovesFixedDistance - && (EncoderCount >= TargetDistanceCount || tMillis > (EncoderTickLastMillis + ENCODER_TICKS_TIMEOUT_MILLIS))) { -#ifdef SUPPORT_RAMP_UP - DebugSpeedAtTargetCountReached = CurrentSpeed; - MotorRampState = MOTOR_STATE_STOPPED; + if (CurrentSpeedPWM > 0) { + if (CheckDistanceInUpdateMotor + && (getDistanceMillimeter() >= TargetDistanceMillimeter + || tMillis > (LastEncoderInterruptMillis + ENCODER_SENSOR_TIMEOUT_MILLIS))) { + stop(MOTOR_BRAKE); // this sets MOTOR_STATE_STOPPED; +#ifdef DEBUG + Serial.print(PWMPin); + if(tMillis > (LastEncoderInterruptMillis + ENCODER_SENSOR_TIMEOUT_MILLIS)){ + Serial.print(F(" Encoder timeout: dist=")); + } else{ + Serial.print(F(" Reached: dist=")); + } + Serial.print(getDistanceMillimeter()); + Serial.print(F(" Breakdist=")); + Serial.println(getBrakingDistanceMillimeter()); #endif - stop(MOTOR_BRAKE); - return false; + return false; // need no more calls to update() } + return true; // still running } - - return true; + return false; // current speed == 0 } /* * Computes motor speed compensation value in order to go exactly straight ahead * Compensate only at forward direction */ -void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned int aCheckInterval) { - if (CurrentDirectionOrBrakeMode != DIRECTION_FORWARD) { +void EncoderMotor::synchronizeMotor(EncoderMotor *aOtherMotorControl, unsigned int aCheckInterval) { + if (CurrentDirectionOrBrakeMode != DIRECTION_FORWARD || aOtherMotorControl->CurrentDirectionOrBrakeMode != DIRECTION_FORWARD) { return; } static long sNextMotorSyncMillis; long tMillis = millis(); if (tMillis >= sNextMotorSyncMillis) { sNextMotorSyncMillis += aCheckInterval; -#ifdef SUPPORT_RAMP_UP // only synchronize if manually operated or at full speed - if ((MotorRampState == MOTOR_STATE_STOPPED && aOtherMotorControl->MotorRampState == MOTOR_STATE_STOPPED && CurrentSpeed > 0) - || (MotorRampState == MOTOR_STATE_DRIVE_SPEED && aOtherMotorControl->MotorRampState == MOTOR_STATE_DRIVE_SPEED)) { -#endif - MotorValuesHaveChanged = false; + if ((MotorRampState == MOTOR_STATE_STOPPED && aOtherMotorControl->MotorRampState == MOTOR_STATE_STOPPED && CurrentSpeedPWM > 0) + || (MotorRampState == MOTOR_STATE_DRIVE && aOtherMotorControl->MotorRampState == MOTOR_STATE_DRIVE)) { + MotorControlValuesHaveChanged = false; if (EncoderCount >= (aOtherMotorControl->EncoderCount + 2)) { EncoderCount = aOtherMotorControl->EncoderCount; /* * This motor is too fast, first try to reduce other motors compensation */ - if (aOtherMotorControl->SpeedCompensation >= 2) { - aOtherMotorControl->SpeedCompensation -= 2; - aOtherMotorControl->CurrentSpeed += 2; - aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; + if (aOtherMotorControl->SpeedPWMCompensation >= 2) { + aOtherMotorControl->SpeedPWMCompensation -= 2; + aOtherMotorControl->CurrentSpeedPWM += 2; + aOtherMotorControl->setSpeedPWM(aOtherMotorControl->CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; EncoderCount = aOtherMotorControl->EncoderCount; - } else if (CurrentSpeed > StartSpeed) { + } else if (CurrentSpeedPWM > StartSpeedPWM) { /* * else increase this motors compensation */ - SpeedCompensation += 2; - CurrentSpeed -= 2; - PWMDcMotor::setSpeed(CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; + SpeedPWMCompensation += 2; + CurrentSpeedPWM -= 2; + PWMDcMotor::setSpeedPWM(CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; } } else if (aOtherMotorControl->EncoderCount >= (EncoderCount + 2)) { @@ -294,28 +329,22 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned /* * Other motor is too fast, first try to reduce this motors compensation */ - if (SpeedCompensation >= 2) { - SpeedCompensation -= 2; - CurrentSpeed += 2; - PWMDcMotor::setSpeed(CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; - } else if (aOtherMotorControl->CurrentSpeed > aOtherMotorControl->StartSpeed) { + if (SpeedPWMCompensation >= 2) { + SpeedPWMCompensation -= 2; + CurrentSpeedPWM += 2; + PWMDcMotor::setSpeedPWM(CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; + } else if (aOtherMotorControl->CurrentSpeedPWM > aOtherMotorControl->StartSpeedPWM) { /* * else increase other motors compensation */ - aOtherMotorControl->SpeedCompensation += 2; - aOtherMotorControl->CurrentSpeed -= 2; - aOtherMotorControl->setSpeed(aOtherMotorControl->CurrentSpeed, DIRECTION_FORWARD); - MotorValuesHaveChanged = true; + aOtherMotorControl->SpeedPWMCompensation += 2; + aOtherMotorControl->CurrentSpeedPWM -= 2; + aOtherMotorControl->setSpeedPWM(aOtherMotorControl->CurrentSpeedPWM, DIRECTION_FORWARD); + MotorControlValuesHaveChanged = true; } } - - if (MotorValuesHaveChanged) { - writeMotorvaluesToEeprom(); - } -#ifdef SUPPORT_RAMP_UP } -#endif } } @@ -325,36 +354,225 @@ void EncoderMotor::synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned /* * Reset all control values as distances, debug values etc. to 0x00 */ -void EncoderMotor::resetControlValues() { - memset(reinterpret_cast(&CurrentVelocity), 0, - (((uint8_t *) &Debug) + sizeof(Debug)) - reinterpret_cast(&CurrentVelocity)); +void EncoderMotor::resetEncoderControlValues() { + memset(reinterpret_cast(&TargetDistanceMillimeter), 0, + (((uint8_t*) &Debug) + sizeof(Debug)) - reinterpret_cast(&TargetDistanceMillimeter)); // to force display of initial values - EncoderTickCounterHasChanged = true; + SensorValuesHaveChanged = true; } /*************************************************** * Encoder functions ***************************************************/ +/* + * Attaches INT0 or INT1 interrupt to this EncoderMotor + * Interrupt is enabled on rising edges + * We can not use both edges since the on and off times of the opto interrupter are too different + * aInterruptNumber can be one of INT0 (at pin D2) or INT1 (at pin D3) for Atmega328 + */ +void EncoderMotor::attachInterrupt(uint8_t aInterruptNumber) { + if (aInterruptNumber > 1) { + return; + } + + if (aInterruptNumber == 0) { + sPointerForInt0ISR = this; +// interrupt on any logical change + EICRA |= (_BV(ISC00) | _BV(ISC01)); +// clear interrupt bit + EIFR |= _BV(INTF0); +// enable interrupt on next change + EIMSK |= _BV(INT0); + } else { + sPointerForInt1ISR = this; + EICRA |= (_BV(ISC10) | _BV(ISC11)); + EIFR |= _BV(INTF1); + EIMSK |= _BV(INT1); + } +} + +/* + * Reset EncoderInterruptDeltaMillis, EncoderInterruptMillisArray, MillisArrayIndex and AverageSpeedIsValid + */ +void EncoderMotor::resetSpeedValues() { +#ifdef SUPPORT_AVERAGE_SPEED + memset((void*) &EncoderInterruptDeltaMillis, 0, + ((uint8_t*) &AverageSpeedIsValid + sizeof(AverageSpeedIsValid)) - (uint8_t*) &EncoderInterruptDeltaMillis); +#else + EncoderInterruptDeltaMillis = 0; +#endif +} + +uint8_t EncoderMotor::getDirection() { + return LastDirection; +} + +unsigned int EncoderMotor::getDistanceMillimeter() { + return LastRideEncoderCount * FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT; +} + +unsigned int EncoderMotor::getDistanceCentimeter() { + return (LastRideEncoderCount * FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT) / 10; +} + +unsigned int EncoderMotor::getBrakingDistanceMillimeter() { + unsigned int tSpeedCmPerSecond = getSpeed(); +// return (tSpeedCmPerSecond * tSpeedCmPerSecond * 100) / RAMP_DECELERATION_TIMES_2; // overflow! + return (tSpeedCmPerSecond * tSpeedCmPerSecond) / (RAMP_DECELERATION_TIMES_2 / 100); +} + +/* + * Speed is in cm/s for a 20 slot encoder disc + * Reset speed values after 1 second + */ +unsigned int EncoderMotor::getSpeed() { + if (millis() - LastEncoderInterruptMillis > 1000) { + resetSpeedValues(); // Reset speed values after 1 second + } + unsigned long tEncoderInterruptDeltaMillis = EncoderInterruptDeltaMillis; + if (tEncoderInterruptDeltaMillis == 0) { + return 0; + } + return (SPEED_SCALE_VALUE / tEncoderInterruptDeltaMillis); +} + +#ifdef SUPPORT_AVERAGE_SPEED +/* + * Speed is in cm/s for a 20 slot encoder disc + * Average is computed over the full revolution to compensate for unequal distances of the laser cut encoder discs. + * If we do not have 21 timestamps, average is computed over the existing ones + */ +unsigned int EncoderMotor::getAverageSpeed() { + int tAverageSpeed = 0; + /* + * First check for timeout + */ + if (millis() - LastEncoderInterruptMillis > 1000) { + resetSpeedValues(); // Reset speed values after 1 second + } else { + int8_t tMillisArrayIndex = MillisArrayIndex; + if (!AverageSpeedIsValid) { + tMillisArrayIndex--; + if (tMillisArrayIndex > 0) { + // here MillisArray is not completely filled and MillisArrayIndex had no wrap around + tAverageSpeed = (SPEED_SCALE_VALUE * tMillisArrayIndex) + / (EncoderInterruptMillisArray[tMillisArrayIndex] - EncoderInterruptMillisArray[0]); + } + } else { + // tMillisArrayIndex points to the next value to write == the oldest value to overwrite + unsigned long tOldestEncoderInterruptMillis = EncoderInterruptMillisArray[tMillisArrayIndex]; + + // get index of current value + tMillisArrayIndex--; + if (tMillisArrayIndex < 0) { + // wrap around + tMillisArrayIndex = AVERAGE_SPEED_BUFFER_SIZE - 1; + } + tAverageSpeed = (SPEED_SCALE_VALUE * AVERAGE_SPEED_SAMPLE_SIZE) + / (EncoderInterruptMillisArray[tMillisArrayIndex] - tOldestEncoderInterruptMillis); + } + } + return tAverageSpeed; +} + +/* + * @param aLengthOfAverage only values from 1 to 20 are valid! + */ +unsigned int EncoderMotor::getAverageSpeed(uint8_t aLengthOfAverage) { + if (!AverageSpeedIsValid && MillisArrayIndex < aLengthOfAverage) { + // cannot compute requested average + return 0; + } + // get index of aLengthOfAverage counts before + int8_t tHistoricIndex = (MillisArrayIndex - 1) - aLengthOfAverage; + if (tHistoricIndex < 0) { + // wrap around + tHistoricIndex += AVERAGE_SPEED_BUFFER_SIZE; + } + int tAverageSpeed = (SPEED_SCALE_VALUE * aLengthOfAverage) + / (LastEncoderInterruptMillis - EncoderInterruptMillisArray[tHistoricIndex]); + + return tAverageSpeed; +} +#endif + +/* + * Print caption for Serial Plotter + * Space but NO println() at the end, to enable additional information to be printed + */ +void EncoderMotor::printEncoderDataCaption(Print *aSerial) { +// aSerial->print(F("PWM[2] Speed[cm/s] SpeedAvg.Of10[cm/s] Count[22cm/20LSB] ")); + aSerial->print(F("PWM[2] Speed[cm/s] Distance[cm] ")); +} + +bool EncoderMotor::printEncoderDataPeriodically(Print *aSerial, uint16_t aPeriodMillis) { + static unsigned long sLastPrintMillis; + + if ((millis() - sLastPrintMillis) >= aPeriodMillis) { // print data every n ms + sLastPrintMillis = millis(); + printEncoderData(aSerial); + return true; + } + return false; +} + +void EncoderMotor::printEncoderData(Print *aSerial) { + aSerial->print(CurrentSpeedPWM / 2); // = PWM, scale it for plotter + aSerial->print(" "); + aSerial->print(getSpeed()); + aSerial->print(" "); +#ifdef SUPPORT_AVERAGE_SPEED +// aSerial->print(getAverageSpeed(10)); +// aSerial->print(" "); +#endif + aSerial->print(getDistanceCentimeter()); + aSerial->print(" "); +} + void EncoderMotor::handleEncoderInterrupt() { long tMillis = millis(); - unsigned int tDeltaMillis = tMillis - EncoderTickLastMillis; - if (tDeltaMillis <= ENCODER_SENSOR_MASK_MILLIS) { -// signal is ringing - CurrentVelocity = 99; + unsigned long tDeltaMillis = tMillis - LastEncoderInterruptMillis; + if (tDeltaMillis <= ENCODER_SENSOR_RING_MILLIS) { + // assume signal is ringing and do nothing } else { - EncoderTickLastMillis = tMillis; + LastEncoderInterruptMillis = tMillis; +#ifdef SUPPORT_AVERAGE_SPEED + uint8_t tMillisArrayIndex = MillisArrayIndex; +#endif + if (tDeltaMillis < ENCODER_SENSOR_TIMEOUT_MILLIS) { + EncoderInterruptDeltaMillis = tDeltaMillis; + } else { + // timeout + EncoderInterruptDeltaMillis = 0; +#ifdef SUPPORT_AVERAGE_SPEED + tMillisArrayIndex = 0; + AverageSpeedIsValid = false; +#endif + } +#ifdef SUPPORT_AVERAGE_SPEED + EncoderInterruptMillisArray[tMillisArrayIndex++] = tMillis; + if (tMillisArrayIndex >= AVERAGE_SPEED_BUFFER_SIZE) { + tMillisArrayIndex = 0; + AverageSpeedIsValid = true; + } + MillisArrayIndex = tMillisArrayIndex; +#endif + EncoderCount++; LastRideEncoderCount++; - CurrentVelocity = VELOCITY_SCALE_VALUE / tDeltaMillis; - EncoderTickCounterHasChanged = true; + SensorValuesHaveChanged = true; } } -// The code for the interrupt is placed at the calling class since we need a fixed relation between ISR and EncoderMotor -// //ISR for PIN PD2 / RIGHT -// ISR(INT0_vect) { -// myCar.rightMotorControl.handleEncoderInterrupt(); -// } +// ISR for PIN PD2 / RIGHT +ISR(INT0_vect) { + sPointerForInt0ISR->handleEncoderInterrupt(); +} + +// ISR for PIN PD3 / LEFT +ISR(INT1_vect) { + sPointerForInt1ISR->handleEncoderInterrupt(); +} /****************************************************************************************** * Static methods @@ -362,39 +580,16 @@ void EncoderMotor::handleEncoderInterrupt() { /* * Enable both interrupts INT0/D2 or INT1/D3 */ -void EncoderMotor::enableINT0AndINT1Interrupts() { +void EncoderMotor::enableINT0AndINT1InterruptsOnRisingEdge() { // interrupt on any logical change - EICRA |= (_BV(ISC00) | _BV(ISC10)); + EICRA |= (_BV(ISC00) | _BV(ISC01) | _BV(ISC10) | _BV(ISC11)); // clear interrupt bit EIFR |= (_BV(INTF0) | _BV(INTF1)); // enable interrupt on next change EIMSK |= (_BV(INT0) | _BV(INT1)); } -/* - * Enable only one interrupt - * aIntPinNumber can be one of INT0/D2 or INT1/D3 for Atmega328 - */ -void EncoderMotor::enableInterruptOnBothEdges(uint8_t aIntPinNumber) { - if (aIntPinNumber > 1) { - return; - } - - if (aIntPinNumber == 0) { -// interrupt on any logical change - EICRA |= (_BV(ISC00)); -// clear interrupt bit - EIFR |= _BV(INTF0); -// enable interrupt on next change - EIMSK |= _BV(INT0); - } else { - EICRA |= (_BV(ISC10)); - EIFR |= _BV(INTF1); - EIMSK |= _BV(INT1); - } -} - #ifdef ENABLE_MOTOR_LIST_FUNCTIONS /* * The list version saves 100 bytes and is more flexible, compared with the array version @@ -408,7 +603,7 @@ EncoderMotor * EncoderMotor::sMotorControlListStart = NULL; *****************************************************/ bool EncoderMotor::updateAllMotors() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list bool tMotorsNotStopped = false; // to check if motors are not stopped by aLoopCallback while (tEncoderMotorControlPointer != NULL) { @@ -421,8 +616,8 @@ bool EncoderMotor::updateAllMotors() { /* * Waits until distance is reached */ -void EncoderMotor::startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; +void EncoderMotor::startRampUpAndWaitForDriveSpeedPWMForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)) { + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { tEncoderMotorControlPointer->startRampUp(aRequestedDirection); @@ -437,11 +632,11 @@ void EncoderMotor::startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirec } while (tMotorsNotStopped && !EncoderMotor::allMotorsStarted()); } -void EncoderMotor::startGoDistanceCountForAll(int aRequestedDistanceCount) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; +void EncoderMotor::startGoDistanceMillimeterForAll(int aRequestedDistanceMillimeter) { + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->startGoDistanceCount(aRequestedDistanceCount); + tEncoderMotorControlPointer->startGoDistanceMillimeter(aRequestedDistanceMillimeter); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } } @@ -449,22 +644,22 @@ void EncoderMotor::startGoDistanceCountForAll(int aRequestedDistanceCount) { /* * Waits until distance is reached */ -void EncoderMotor::goDistanceCountForAll(int aRequestedDistanceCount, void (*aLoopCallback)(void)) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; +void EncoderMotor::goDistanceMillimeterForAll(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)) { + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->startGoDistanceCount(aRequestedDistanceCount); + tEncoderMotorControlPointer->startGoDistanceMillimeter(aRequestedDistanceMillimeter); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } waitUntilAllMotorsStopped(aLoopCallback); } bool EncoderMotor::allMotorsStarted() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; bool tAllAreStarted = true; // walk through list while (tEncoderMotorControlPointer != NULL) { - if (tEncoderMotorControlPointer->MotorRampState != MOTOR_STATE_DRIVE_SPEED) { + if (tEncoderMotorControlPointer->MotorRampState != MOTOR_STATE_DRIVE) { tAllAreStarted = false; } tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; @@ -473,7 +668,7 @@ bool EncoderMotor::allMotorsStarted() { } bool EncoderMotor::allMotorsStopped() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; bool tAllAreStopped = true; // walk through list while (tEncoderMotorControlPointer != NULL) { @@ -486,17 +681,13 @@ bool EncoderMotor::allMotorsStopped() { } /* - * Set NextChangeMaxTargetCount to change state from MOTOR_STATE_DRIVE_SPEED to MOTOR_STATE_RAMP_DOWN - * Use DistanceCountAfterRampUp as ramp down count - * Busy waits for stop + * Start ramp down and busy wait for stop */ void EncoderMotor::stopAllMotorsAndWaitUntilStopped() { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { - tEncoderMotorControlPointer->NextChangeMaxTargetCount = tEncoderMotorControlPointer->EncoderCount; - tEncoderMotorControlPointer->TargetDistanceCount = tEncoderMotorControlPointer->EncoderCount - + tEncoderMotorControlPointer->DistanceCountAfterRampUp; + tEncoderMotorControlPointer->startRampDown(); tEncoderMotorControlPointer = tEncoderMotorControlPointer->NextMotorControl; } @@ -518,7 +709,7 @@ void EncoderMotor::waitUntilAllMotorsStopped(void (*aLoopCallback)(void)) { } void EncoderMotor::stopAllMotors(uint8_t aStopMode) { - EncoderMotor * tEncoderMotorControlPointer = sMotorControlListStart; + EncoderMotor *tEncoderMotorControlPointer = sMotorControlListStart; // walk through list while (tEncoderMotorControlPointer != NULL) { tEncoderMotorControlPointer->stop(aStopMode); @@ -526,3 +717,4 @@ void EncoderMotor::stopAllMotors(uint8_t aStopMode) { } } #endif +#endif diff --git a/src/EncoderMotor.h b/src/EncoderMotor.h index c133415..649aa8a 100644 --- a/src/EncoderMotor.h +++ b/src/EncoderMotor.h @@ -1,12 +1,17 @@ /* * EncoderMotor.h * - * Created on: 16.09.2016 - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Created on: 12.05.2019 + * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -22,52 +27,85 @@ #include "PWMDcMotor.h" #include +#define SUPPORT_AVERAGE_SPEED +#define AVERAGE_SPEED_SAMPLE_SIZE 20 +#define AVERAGE_SPEED_BUFFER_SIZE 21 // one more than samples, because speed is the difference between 2 samples + // maybe useful especially for more than 2 motors //#define ENABLE_MOTOR_LIST_FUNCTIONS /* - * Encoders generate 110 Hz at max speed => 8 ms per period - * since duty cycle of encoder disk impulse is 1/3 choose 3 millis as ringing mask. - * 3 ms means 2.1 to 3.0 ms depending on the Arduino ms trigger. + * 20 slot Encoder generates 4 to 5 Hz at min speed and 110 Hz at max speed => 200 to 8 ms per period + */ +#define ENCODER_COUNTS_PER_FULL_ROTATION 20 +#define ENCODER_SENSOR_TIMEOUT_MILLIS 400L // Timeout for encoder ticks if motor is running +#define ENCODER_SENSOR_RING_MILLIS 4 + +/* + * Some factors depending on wheel diameter and encoder resolution + */ +#if ! defined(FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT) +// Exact value is 220 mm / 20 = 11 +#define FACTOR_COUNT_TO_MILLIMETER_INTEGER_DEFAULT ((DEFAULT_CIRCUMFERENCE_MILLIMETER + (ENCODER_COUNTS_PER_FULL_ROTATION / 2)) / ENCODER_COUNTS_PER_FULL_ROTATION) // = 11 +#endif + +/* + * The millis per tick have the unit [ms]/ (circumference[cm]/countsPerCircumference) -> ms/cm + * To get cm/s, use (circumference[cm]/countsPerCircumference) * 1000 / millis per tick */ -#define ENCODER_SENSOR_MASK_MILLIS 3 -#define VELOCITY_SCALE_VALUE 500 // for computing of CurrentVelocity -// Timeout for encoder ticks if motor is running -#define ENCODER_TICKS_TIMEOUT_MILLIS 500 +#define SPEED_SCALE_VALUE ((100L * DEFAULT_CIRCUMFERENCE_MILLIMETER) / ENCODER_COUNTS_PER_FULL_ROTATION) // 1100 class EncoderMotor: public PWMDcMotor { public: EncoderMotor(); #ifdef USE_ADAFRUIT_MOTOR_SHIELD - void init(uint8_t aMotorNumber, bool aReadFromEeprom = false); + void init(uint8_t aMotorNumber); + void init(uint8_t aMotorNumber, uint8_t aInterruptNumber); #else - void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber = 0); + void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin); + void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aInterruptNumber); #endif // virtual ~EncoderMotor(); /* * Functions for going a fixed distance, they "overwrite" PWMDCMotor functions */ - void startGoDistanceCount(int aRequestedDistanceCount); // Signed distance count - void startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); - void startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); + void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // Signed distance count + void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); + void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); bool updateMotor(); /* * Functions especially for encoder motors */ - void synchronizeMotor(EncoderMotor * aOtherMotorControl, unsigned int aCheckInterval); // Computes motor speed compensation value in order to go exactly straight ahead - static void calibrate(); // Generates a rising ramp and detects the first movement -> this sets StartSpeed / dead band + void synchronizeMotor(EncoderMotor *aOtherMotorControl, unsigned int aCheckInterval); // Computes motor speed compensation value in order to go exactly straight ahead + static void calibrate(); // Generates a rising ramp and detects the first movement -> this sets StartSpeedPWM / dead band /* * Encoder interrupt handling */ void handleEncoderInterrupt(); - static void enableINT0AndINT1Interrupts(); - static void enableInterruptOnBothEdges(uint8_t aIntPinNumber); + void attachInterrupt(uint8_t aInterruptPinNumber); + static void enableINT0AndINT1InterruptsOnRisingEdge(); + + uint8_t getDirection(); + unsigned int getDistanceMillimeter(); + unsigned int getDistanceCentimeter(); + unsigned int getBrakingDistanceMillimeter(); + + unsigned int getSpeed(); +#ifdef SUPPORT_AVERAGE_SPEED + unsigned int getAverageSpeed(); + unsigned int getAverageSpeed(uint8_t aLengthOfAverage); +#endif + + void printEncoderDataCaption(Print *aSerial); + bool printEncoderDataPeriodically(Print *aSerial, uint16_t aPeriodMillis); + void printEncoderData(Print *aSerial); - void resetControlValues(); // Shutdown and reset all control values and sets direction to forward + void resetEncoderControlValues(); + void resetSpeedValues(); #ifdef ENABLE_MOTOR_LIST_FUNCTIONS /* @@ -75,9 +113,9 @@ class EncoderMotor: public PWMDcMotor { */ static bool updateAllMotors(); - static void startGoDistanceCountForAll(int aRequestedDistanceCount); - static void goDistanceCountForAll(int aRequestedDistanceCount, void (*aLoopCallback)(void)); - static void startRampUpAndWaitForDriveSpeedForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)); + static void startGoDistanceMillimeterForAll(int aRequestedDistanceMillimeter); + static void goDistanceMillimeterForAll(int aRequestedDistanceMillimeter, void (*aLoopCallback)(void)); + static void startRampUpAndWaitForDriveSpeedPWMForAll(uint8_t aRequestedDirection, void (*aLoopCallback)(void)); static void stopAllMotors(uint8_t aStopMode); static void waitUntilAllMotorsStopped(void (*aLoopCallback)(void)); @@ -95,40 +133,32 @@ class EncoderMotor: public PWMDcMotor { EncoderMotor * NextMotorControl; #endif + /************************************************************** + * Variables required for going a fixed distance with encoder + **************************************************************/ /* - * Flags for display update control - */ - volatile static bool EncoderTickCounterHasChanged; - - /********************************************************** - * Variables required for going a fixed distance - **********************************************************/ - /* - * Reset() resets all members from CurrentVelocity to (including) Debug to 0 + * Reset() resets all members from TargetDistanceCount to (including) Debug to 0 */ - int CurrentVelocity; // VELOCITY_SCALE_VALUE / tDeltaMillis - - unsigned int TargetDistanceCount; - unsigned int LastTargetDistanceCount; + unsigned int TargetDistanceMillimeter; + unsigned int LastTargetDistanceMillimeter; /* * Distance optocoupler impulse counter. It is reset at startGoDistanceCount if motor was stopped. */ volatile unsigned int EncoderCount; volatile unsigned int LastRideEncoderCount; // count of last ride - from start of MOTOR_STATE_RAMP_UP to next MOTOR_STATE_RAMP_UP - // The next value is volatile, but volatile increases the code size by 20 bites without any logical improvement - unsigned long EncoderTickLastMillis; // used for debouncing and lock/timeout detection + // Flag e.g. for display update control + volatile unsigned long LastEncoderInterruptMillis; // used internal for debouncing and lock/timeout detection -#ifdef SUPPORT_RAMP_UP /* - * For ramp and state control + * for speed computation + * Do not rearrange, since reset is done with memset(). */ - uint8_t RampDeltaPerDistanceCount; - unsigned int NextChangeMaxTargetCount; // target count at which next change must be done - - uint8_t DistanceCountAfterRampUp; // number of ticks at the transition from MOTOR_STATE_RAMP_UP to MOTOR_STATE_FULL_SPEED to be used for computing ramp down start ticks - unsigned int DebugCount; // for different debug purposes of ramp optimisation - uint8_t DebugSpeedAtTargetCountReached; // for debug of ramp down + volatile unsigned long EncoderInterruptDeltaMillis; // Used to get speed +#ifdef SUPPORT_AVERAGE_SPEED + volatile unsigned int EncoderInterruptMillisArray[AVERAGE_SPEED_BUFFER_SIZE]; // store for 20 deltas + volatile uint8_t MillisArrayIndex; // Index of the next value to write == the oldest value to overwrite. 0 to 20|(AVERAGE_SPEED_BUFFER_SIZE-1) + volatile bool AverageSpeedIsValid; // true if 11 values are written since last timeout #endif // do not delete it!!! It must be the last element in structure and is required for stopMotorAndReset() @@ -136,6 +166,9 @@ class EncoderMotor: public PWMDcMotor { }; +extern EncoderMotor * sPointerForInt0ISR; +extern EncoderMotor * sPointerForInt1ISR; + #endif /* SRC_ENCODERMOTORCONTROL_H_ */ #pragma once diff --git a/src/HCSR04.cpp b/src/HCSR04.cpp index 2e9d3d9..caabc11 100644 --- a/src/HCSR04.cpp +++ b/src/HCSR04.cpp @@ -112,7 +112,8 @@ unsigned int getUSDistance(unsigned int aTimeoutMicros) { uint8_t tEchoInPin; if (sHCSR04Mode == HCSR04_MODE_USE_1_PIN) { - delayMicroseconds(10); // allow for 10 us low before switching to input which is high because of the modules pullup resistor. + // allow for 20 us low (20 us instead of 10 us also supports the JSN-SR04T) before switching to input which is high because of the modules pullup resistor. + delayMicroseconds(20); pinMode(sTriggerOutPin, INPUT); tEchoInPin = sTriggerOutPin; } else { @@ -185,7 +186,7 @@ void testUSSensor(uint16_t aSecondsToTest) { * Result is in sUSDistanceCentimeter; */ -// Comment out the line according to the sEchoInPin if using the non blocking version +// Activate the line according to the sEchoInPin if using the non blocking version // or define it as symbol for the compiler e.g. -DUSE_PIN_CHANGE_INTERRUPT_D0_TO_D7 //#define USE_PIN_CHANGE_INTERRUPT_D0_TO_D7 // using PCINT2_vect - PORT D //#define USE_PIN_CHANGE_INTERRUPT_D8_TO_D13 // using PCINT0_vect - PORT B - Pin 13 is feedback output diff --git a/src/HomePage.cpp b/src/HomePage.cpp index 1a3e40b..52c5263 100644 --- a/src/HomePage.cpp +++ b/src/HomePage.cpp @@ -54,13 +54,13 @@ BDSlider SliderTilt; // Here we get values from 0 to 180 degrees from scaled slider #ifdef CAR_HAS_PAN_SERVO -void doHorizontalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doHorizontalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue) { PanServo.write(aValue); } #endif #ifdef CAR_HAS_TILT_SERVO -void doVerticalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doVerticalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue) { TiltServo.write(aValue); } #endif @@ -85,31 +85,31 @@ void doPlayMelody(BDButton * aTheTouchedButton, int16_t aValue) { void initHomePage(void) { - TouchButtonTestPage.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, F("Test"), - TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_TEST, &GUISwitchPages); + TouchButtonBTSensorDrivePage.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, + F("Sensor\nDrive"), TEXT_SIZE_18, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_BT_SENSOR_CONTROL, &GUISwitchPages); - TouchButtonAutomaticDrivePage.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, F("Automatic\nControl"), - TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_AUTOMATIC_CONTROL, &GUISwitchPages); + TouchButtonAutomaticDrivePage.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, + F("Automatic\nControl"), TEXT_SIZE_16, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_AUTOMATIC_CONTROL, &GUISwitchPages); - TouchButtonBTSensorDrivePage.init(BUTTON_WIDTH_3_POS_3, - BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_RED, - F("Sensor drive"), TEXT_SIZE_12, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_BT_SENSOR_CONTROL, &GUISwitchPages); + TouchButtonTestPage.init(BUTTON_WIDTH_3_POS_3, + BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_RED, + F("Test"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_TEST, &GUISwitchPages); #ifdef CAR_HAS_CAMERA - TouchButtonCameraOnOff.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_4, - TEXT_SIZE_22_HEIGHT, COLOR_BLACK, F("Camera"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, + TouchButtonCameraOnOff.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, + TEXT_SIZE_22_HEIGHT, COLOR16_BLACK, F("Cam"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doCameraSupplyOnOff); #endif #ifdef ENABLE_RTTTL TouchButtonMelody.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, BUTTON_HEIGHT_8, COLOR_BLACK, F("Melody"), TEXT_SIZE_22, + BUTTON_WIDTH_3, BUTTON_HEIGHT_8, COLOR16_BLACK, F("Melody"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, sPlayMelody, &doPlayMelody); #endif #ifdef CAR_HAS_LASER TouchButtonLaser.init(0, BUTTON_HEIGHT_4_LINE_4 - (TEXT_SIZE_22_HEIGHT + BUTTON_DEFAULT_SPACING_QUARTER), - BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR_BLACK, F("Laser"), TEXT_SIZE_22, + BUTTON_WIDTH_3, TEXT_SIZE_22_HEIGHT, COLOR16_BLACK, F("Laser"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doLaserOnOff); #endif @@ -127,8 +127,12 @@ void drawHomePage(void) { #else char tCarTypeString[] = "2WD"; #endif - - BlueDisplay1.drawText(HEADER_X + (3 * TEXT_SIZE_22_WIDTH), (3 * TEXT_SIZE_22_HEIGHT), tCarTypeString); +#ifdef CAR_HAS_CAMERA + BlueDisplay1.drawText(HEADER_X + (2 * TEXT_SIZE_22_WIDTH), (2 * TEXT_SIZE_22_HEIGHT) + TEXT_SIZE_11_HEIGHT - 2, tCarTypeString, + TEXT_SIZE_11, COLOR16_RED, COLOR16_NO_BACKGROUND); +#else + BlueDisplay1.drawText(HEADER_X + (2 * TEXT_SIZE_22_WIDTH), (3 * TEXT_SIZE_22_HEIGHT), tCarTypeString); +#endif TouchButtonRobotCarStartStop.drawButton(); #ifdef CAR_HAS_CAMERA @@ -145,12 +149,14 @@ void drawHomePage(void) { TouchButtonAutomaticDrivePage.drawButton(); TouchButtonDirection.drawButton(); -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) TouchButtonCalibrate.drawButton(); #endif TouchButtonCompensationLeft.drawButton(); TouchButtonCompensationRight.drawButton(); +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonCompensationStore.drawButton(); +#endif SliderUSPosition.setValueAndDrawBar(sLastServoAngleInDegrees); SliderUSPosition.drawSlider(); @@ -173,13 +179,16 @@ void drawHomePage(void) { SliderSpeedRight.drawSlider(); SliderSpeedLeft.drawSlider(); #endif - PWMDcMotor::MotorValuesHaveChanged = true; // trigger drawing of values + PWMDcMotor::MotorControlValuesHaveChanged = true; // trigger drawing of values } void startHomePage(void) { - TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4); -#ifdef USE_ENCODER_MOTOR_CONTROL - TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_4); + TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_5); +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3); +#endif +#if defined(CAR_HAS_TILT_SERVO) && defined(SUPPORT_EEPROM_STORAGE) + TouchButtonCompensationStore.setPosition(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5); #endif drawHomePage(); } @@ -189,8 +198,11 @@ void loopHomePage(void) { void stopHomePage(void) { TouchButtonDirection.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_6); -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) TouchButtonCalibrate.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2); +#endif +#if defined(CAR_HAS_TILT_SERVO) && defined(SUPPORT_EEPROM_STORAGE) + TouchButtonCompensationStore.setPosition(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_4); #endif startStopRobotCar(false); } diff --git a/src/IMUCarData.cpp b/src/IMUCarData.cpp new file mode 100644 index 0000000..37527c8 --- /dev/null +++ b/src/IMUCarData.cpp @@ -0,0 +1,539 @@ +/* + * CarIMUData.cpp + * + * Functions for getting IMU data from MPU6050 for car control. + * + * Created on: 19.11.2020 + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include "Wire.h" +#include "IMUCarData.h" + +#define FIFO_NUMBER_OF_ACCEL_VALUES 3 +#define FIFO_NUMBER_OF_GYRO_VALUES 1 +#define FIFO_CHUNK_SIZE ((FIFO_NUMBER_OF_ACCEL_VALUES + FIFO_NUMBER_OF_GYRO_VALUES) * 2) + +//#define DEBUG_WITHOUT_SERIAL // For debugging without Serial development +#ifdef DEBUG_WITHOUT_SERIAL +#include "DigitalWriteFast.h" +#endif +//#define DEBUG // Only for development +//#define WARN // Information that the program may encounter problems, like small Heap/Stack area. +//#define AUTO_OFFSET_DEBUG // Show auto offset values as dummy values for the plotter output +#include "DebugLevel.h" // Include to propagate debug levels + +int8_t IMUCarData::getAcceleratorForward15MilliG() { + return AcceleratorForward.Byte.HighByte; +} + +int8_t IMUCarData::getAcceleratorForward4MilliG() { + return AcceleratorForward.Word >> 6; // /64 -> 256 per g +} + +int8_t IMUCarData::getAcceleratorForwardLowPass8() { +// return AcceleratorForwardLowPass8.Byte.HighByte; + return AcceleratorForwardLowPass8.Word.HighWord >> 6; // 256 per g +} + +int8_t IMUCarData::getAcceleratorForwardLowPass4() { + return AcceleratorForwardLowPass4.Word.HighWord >> 6; // 256 per g +} + +// shift 14 -> 1 (0.981) cm/s , 12 -> 2.5 mm/s, 8 -> 0.15328 mm/s +int IMUCarData::getSpeedCmPerSecond() { + return Speed.Long >> (14 - RATE_SHIFT); +} +/* + * 0.15 mm/s resolution for 1kHz sampling rate + */ +int IMUCarData::getSpeedFastWithHigherResolution() { + return Speed.Long >> 8; +} + +/* + * Distance is the sum of Speed >> 8. + * At 1 kHz: with 1 cm/s (shift (14-8)) we have 1 cm (or shift (24-8)) after 1 second (1000 additions) + */ +int IMUCarData::getDistanceCm() { +#if SAMPLE_RATE == 1000 + return Distance.Word.HighWord; // 0.958 cm since we shift by 10 and do not divide by 1000 +#else + return Distance.Long >> (16 - (2 * RATE_SHIFT)); // 0.958 cm since we shift by additional 10 and do not divide by 1000 +#endif +} + +int IMUCarData::getDistanceMillimeter() { +#if SAMPLE_RATE == 1000 + LongUnion tDistance; + tDistance.Long = Distance.Long * 10; // use full resolution for * 10 + return tDistance.Word.HighWord; // saves 10 bytes +// return ((Distance.Long * 10 ) >> 16) ; // 0.958 mm +#else + return (Distance.Long * 10) >> (16 - (2 * RATE_SHIFT)); // 0.958 cm since we shift by additional 10 and do not divide by 1000 +#endif +} + +/* + * Gyroscope + */ +int IMUCarData::getGyroscopePan2DegreePerSecond() { + return GyroscopePan.Byte.HighByte; +} + +int IMUCarData::getTurnAngleHalfDegree() { +#if SAMPLE_RATE == 1000 + return TurnAngle.Word.HighWord; +#else + return TurnAngle.Long >> (16 - RATE_SHIFT); +#endif +} + +int IMUCarData::getTurnAngleDegree() { +#if SAMPLE_RATE == 1000 + return TurnAngle.Word.HighWord >> 1; +#elif SAMPLE_RATE == 500 + return TurnAngle.Word.HighWord; +#else + return TurnAngle.Long >> (16 - (RATE_SHIFT - 1)); +#endif +} + +/* + * Print caption for Serial Plotter + * Space but NO println() at the end, to enable additional information to be printed + */ +void IMUCarData::printIMUCarDataCaption(Print *aSerial) { +// aSerial->print(F("AccelForward[4mg/LSB] AccelForwardLP8 Speed[cm/s] Distance[cm] GyroZ[2dps/LSB] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +// aSerial->print(F("AccelForward[4mg/LSB] AccelForwardAverage AccelForwardLP8 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +// aSerial->print(F("AccelAvg.[4mg/LSB] AccelLP8 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +// aSerial->print(F("AccelLP8 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range + aSerial->print(F("AccelLP4 Speed[cm/s] Distance[cm] AngleZ[0.5d/LSB] ")); // +/-250 | 500 degree per second for 16 bit full range +} + +/* + * read from FIFO and does auto offset (if enabled) + */ +void IMUCarData::delayAndReadIMUCarDataData(unsigned long aMillisDelay) { + while (aMillisDelay > 0) { + readCarDataFromMPU6050Fifo(); + delay(DELAY_TO_NEXT_IMU_DATA_MILLIS); + aMillisDelay -= DELAY_TO_NEXT_IMU_DATA_MILLIS; + } +} + +bool IMUCarData::printIMUCarDataDataPeriodically(Print *aSerial, uint16_t aPeriodMillis) { + static unsigned long sLastPrintMillis; + + if ((millis() - sLastPrintMillis) >= aPeriodMillis) { // print data every n ms + sLastPrintMillis = millis(); + readCarDataFromMPU6050Fifo(); + printIMUCarData(aSerial); + return true; + } + return false; +} + +void IMUCarData::printIMUCarData(Print *aSerial) { +// aSerial->print(getAcceleratorForward4MilliG()); +// aSerial->print(" "); + // Too noisy +// aSerial->print(getAcceleratorForward4MilliG()); +// aSerial->print(" "); + aSerial->print(getAcceleratorForwardLowPass4()); + aSerial->print(" "); + aSerial->print(abs(getSpeedCmPerSecond())); + aSerial->print(" "); + aSerial->print(abs(getDistanceCm())); + aSerial->print(" "); +// aSerial->print(getGyroscopePan2DegreePerSecond()); +// aSerial->print(" "); + aSerial->print(getTurnAngleHalfDegree()); + aSerial->print(" "); +} + +unsigned int IMUCarData::getMPU6050SampleRate() { + return (SAMPLE_RATE); +} + +/* + * Read raw 14 vales. Requires 500 us. + * Sets AcceleratorForward and GyroscopePan + */ +void IMUCarData::readCarDataFromMPU6050() { + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(MPU6050_RA_ACCEL_XOUT_H); + Wire.endTransmission(false); + Wire.requestFrom((uint8_t) MPU6050_DEFAULT_ADDRESS, (uint8_t) 14, (uint8_t) true); + +#ifdef USE_ACCELERATOR_Y_FOR_SPEED +// skip x value + Wire.read(); + Wire.read(); +#endif +// read forward value + AcceleratorForward.Byte.HighByte = Wire.read(); + AcceleratorForward.Byte.LowByte = Wire.read(); +#ifdef USE_NEGATIVE_ACCELERATION_FOR_SPEED + AcceleratorForward.Word = (-AcceleratorForward.Word) - AcceleratorForwardOffset; +#else + AcceleratorForward.Word = AcceleratorForward.Word - AcceleratorForwardOffset; +#endif + + AcceleratorForwardLowPass8.Long += ((((int32_t) AcceleratorForward.Word) << 16) - AcceleratorForwardLowPass8.Long) >> 8; // Fixed point 2.0 us + AcceleratorForwardLowPass4.Long += ((((int32_t) AcceleratorForward.Word) << 16) - AcceleratorForwardLowPass4.Long) >> 4; // Fixed point 2.0 us + Speed.Long += AcceleratorForward.Word; + Distance.Long += Speed.Long >> 8; + +#ifndef USE_ACCELERATOR_Y_FOR_SPEED + // skip y value + Wire.read(); + Wire.read(); +#endif + +// skip z, temp and 2 gyroscope values value + for (uint8_t i = 0; i < 8; i++) { + Wire.read(); + } + +// read pan (Z) value + GyroscopePan.Byte.HighByte = Wire.read(); + GyroscopePan.Byte.LowByte = Wire.read(); + GyroscopePan.Word -= GyroscopePanOffset; + TurnAngle.Long += GyroscopePan.Word; +} + +/* + * @return true, if data might have changed + */ +bool IMUCarData::readCarDataFromMPU6050Fifo() { +#ifdef DEBUG_WITHOUT_SERIAL + digitalToggleFast(12); +#endif + if (millis() - LastFifoCheckMillis < DELAY_TO_NEXT_IMU_DATA_MILLIS) { + return false; // no new data expected + } + + /* + * Read FIFO count only once at start of the function + */ + uint16_t tFifoCount = MPU6050ReadWordSwapped(MPU6050_RA_FIFO_COUNTH); + LastFifoCheckMillis = millis(); + if (tFifoCount == 1024) { + resetMPU6050FifoAndCarData(); + return true; + } + /* + * 660 /1200 us @400kHz for 16 / 32 byte transfer. + * 140 us between each block transfer + * 1000 us for next 32 bytes transfer => ~3.1 ms for 10 chunks + */ +#ifdef DEBUG + Serial.print(tFifoCount); + Serial.print("|"); +#endif + int32_t tAcceleratorForward = 0; + int32_t tGyroscopePan = 0; + int8_t tReadChunckCount = 0; + while (tFifoCount >= FIFO_CHUNK_SIZE) { + /* + * Use only multiple of FIFO_CHUNK_SIZE as read length and clip read length to I2C BUFFER_LENGTH + */ + uint8_t tChunkCount = tFifoCount / FIFO_CHUNK_SIZE; + if (tChunkCount > BUFFER_LENGTH / FIFO_CHUNK_SIZE) { + tChunkCount = BUFFER_LENGTH / FIFO_CHUNK_SIZE; // we can get 4 chunks of data with the 32 byte buffer of the Wire library + } + // this reads max 28 bytes or 4 chunks + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(MPU6050_RA_FIFO_R_W); + Wire.endTransmission(false); + uint8_t tReceivedCount = Wire.requestFrom((uint8_t) MPU6050_DEFAULT_ADDRESS, (uint8_t) (tChunkCount * FIFO_CHUNK_SIZE), + (uint8_t) true); +#ifdef WARN + if (tReceivedCount != (tChunkCount * FIFO_CHUNK_SIZE)) { + // should never happen + Serial.print(tReceivedCount); + Serial.print(F(" received bytes are not equal requested quantity of ")); + Serial.println(tChunkCount * FIFO_CHUNK_SIZE); + } +#endif + for (uint8_t j = 0; j < tReceivedCount; j += FIFO_CHUNK_SIZE) { + // we must read all 3 values + for (uint8_t i = 0; i < FIFO_NUMBER_OF_ACCEL_VALUES; i++) { + WordUnion tAcceleratorValue; + tAcceleratorValue.Byte.HighByte = Wire.read(); + tAcceleratorValue.Byte.LowByte = Wire.read(); + // process only forward value +#ifdef USE_ACCELERATOR_Y_FOR_SPEED + if (i == 1) { +#else + if (i == 0) { +#endif +#ifdef USE_NEGATIVE_ACCELERATION_FOR_SPEED + tAcceleratorValue.Word = (-tAcceleratorValue.Word) - AcceleratorForwardOffset; +#else + tAcceleratorValue.Word = tAcceleratorValue.Word - AcceleratorForwardOffset; +#endif + tAcceleratorForward += tAcceleratorValue.Word; + AcceleratorForwardLowPass8.Long += + ((((int32_t) tAcceleratorValue.Word) << 16) - AcceleratorForwardLowPass8.Long) >> 8; // Fixed point 2.0 us + AcceleratorForwardLowPass4.Long += + ((((int32_t) tAcceleratorValue.Word) << 16) - AcceleratorForwardLowPass4.Long) >> 4; // Fixed point 2.0 us + + Speed.Long += tAcceleratorValue.Word; + Distance.Long += Speed.Long >> 8; + tReadChunckCount++; + } + } + WordUnion tValue; + tValue.Byte.HighByte = Wire.read(); + tValue.Byte.LowByte = Wire.read(); + tValue.Word = tValue.Word - GyroscopePanOffset; + tGyroscopePan += tValue.Word; + // Compute turn angle + TurnAngle.Long += tValue.Word; + } + tFifoCount -= tReceivedCount; + + } // while tFifoCount >= FIFO_CHUNK_SIZE + sCountOfUndisturbedFifoChunks += tReadChunckCount; + // compute average of read values + AcceleratorForward.Word = tAcceleratorForward / (int32_t) tReadChunckCount; + GyroscopePan.Word = tGyroscopePan / (int32_t) tReadChunckCount; + + /* + * Get initial offset values + */ + if (AcceleratorForwardOffset == 0 && sCountOfUndisturbedFifoChunks >= NUMBER_OF_OFFSET_CALIBRATION_SAMPLES) { + /* + * Take the first NUMBER_OF_OFFSET_CALIBRATION_SAMPLES values for auto offset + * My experience is, that the Gyro offset is changing in the first seconds after power up and needs a recalibration + */ + AcceleratorForwardOffset = Speed.Long / sCountOfUndisturbedFifoChunks; + GyroscopePanOffset = TurnAngle.Long / sCountOfUndisturbedFifoChunks; + OffsetsHaveChanged = true; + resetCarData(); // reset temporarily used values +#ifdef DEBUG + printSpeedAndTurnOffsets(&Serial); +#endif + } + +#ifndef DISABLE_AUTO_OFFSET + doAutoOffset(); +#endif + return true; +} + +/* + * Automatic offset acquisition 100 ms after boot and offset correction if no acceleration takes place + */ +void IMUCarData::doAutoOffset() { + // do not auto adjust if we have no initial offsets, which are required for decision if sensor has not moved + if (AcceleratorForwardOffset != 0) { + /* + * Adjust Offsets, if sensor has not moved after NUMBER_OF_OFFSET_CALIBRATION_SAMPLES + * Requires 420 bytes FLASH + */ + int16_t tAccelDifference = abs(AcceleratorForwardLowPass8.Word.HighWord - AcceleratorForward.Word); + if (tAccelDifference > ACCEL_MOVE_THRESHOLD || GyroscopePan.Word < -(GYRO_MOVE_THRESHOLD) + || GYRO_MOVE_THRESHOLD < GyroscopePan.Word) { // using abs() costs 6 byte FLASH +#ifdef AUTO_OFFSET_DEBUG + // just to show the removed deltas in Arduino Plotter + if (AcceleratorForward.Word < -(ACCEL_MOVE_THRESHOLD) || ACCEL_MOVE_THRESHOLD < AcceleratorForward.Word) { + AcceleratorForward.Word = 64 * 10 * ((Speed.Long - sSpeedSnapshot) / sCountOfUndisturbedFifoChunks); + } else { + GyroscopePan.Word = 10 * 256 * ((TurnAngle.Long - sTurnSnapshot) / sCountOfUndisturbedFifoChunks); + } +#endif + sCountOfUndisturbedFifoChunks = 0; // reset count + sSpeedSnapshot = Speed.Long; + sTurnSnapshot = TurnAngle.Long; + } else { + /* + * Do we have enough samples for auto offset? + */ + if (sCountOfUndisturbedFifoChunks >= NUMBER_OF_OFFSET_CALIBRATION_SAMPLES) { + if (abs(Speed.Long - sSpeedSnapshot) > sCountOfUndisturbedFifoChunks) { + // difference is higher than 1 per sample, so adjust accelerator offset + AcceleratorForwardOffset += (Speed.Long - sSpeedSnapshot) / sCountOfUndisturbedFifoChunks; + OffsetsHaveChanged = true; +#ifdef AUTO_OFFSET_DEBUG + // just to show in Arduino Plotter - 5 for each accelerator offset increment + AcceleratorForward.Word = 64 * 5 * ((Speed.Long - sSpeedSnapshot) / sCountOfUndisturbedFifoChunks); +#endif + sSpeedSnapshot = 0; + Speed.Long = 0; + Distance.Long = 0; + + sCountOfUndisturbedFifoChunks = 0; // reset count + } + + if (abs(TurnAngle.Long - sTurnSnapshot) > sCountOfUndisturbedFifoChunks) { + // adjust gyroscope offset and reset gyroscope to last value + GyroscopePanOffset += (TurnAngle.Long - sTurnSnapshot) / sCountOfUndisturbedFifoChunks; + OffsetsHaveChanged = true; +#ifdef AUTO_OFFSET_DEBUG + // just to show in Arduino Plotter - 5 for each gyroscope offset increment + GyroscopePan.Word = 5 * 256 * ((TurnAngle.Long - sTurnSnapshot) / sCountOfUndisturbedFifoChunks); +#endif + TurnAngle.Long = sTurnSnapshot; + + sCountOfUndisturbedFifoChunks = 0; // reset count + } + } + } + } +} + +/* + * Read all data at 1 kHz rate direct from registers, not from FIFO + * and compute and store only the required values in one turn. + */ +void IMUCarData::calculateSpeedAndTurnOffsets() { + + resetCarData(); + AcceleratorForwardOffset = 0; + GyroscopePanOffset = 0; + + uint32_t LastDataMillis = millis(); + for (int i = 0; i < NUMBER_OF_OFFSET_CALIBRATION_SAMPLES; i++) { + // get data every ms + while (millis() == LastDataMillis) { + ; + } + LastDataMillis = millis(); + readCarDataFromMPU6050(); + } + AcceleratorForwardOffset = Speed.Long / NUMBER_OF_OFFSET_CALIBRATION_SAMPLES; + GyroscopePanOffset = TurnAngle.Long / NUMBER_OF_OFFSET_CALIBRATION_SAMPLES; + + resetCarData(); +} + +void IMUCarData::printSpeedAndTurnOffsets(Print *aSerial) { + aSerial->print(F("Speed offset=")); + aSerial->print(AcceleratorForwardOffset); + aSerial->print('|'); + aSerial->print(AcceleratorForwardOffset * ACCEL_RAW_TO_G_FOR_2G_RANGE); + aSerial->print(F(" turn offset=")); + aSerial->print(GyroscopePanOffset); + aSerial->print('|'); + aSerial->println(GyroscopePanOffset * GYRO_RAW_TO_DEGREE_PER_SECOND_FOR_250DPS_RANGE); +} + +void IMUCarData::reset() { + initMPU6050FifoForCarData(); // resets also CarData +} + +void IMUCarData::resetCarData() { + AcceleratorForwardLowPass8.ULong = 0; + AcceleratorForwardLowPass4.ULong = 0; + Speed.ULong = 0; + Distance.Long = 0; + TurnAngle.ULong = 0; +} + +void IMUCarData::resetOffsetData() { + /* + * Reset both offset data, since they are used to comupte the new sums, which in turn gives the new offsets + */ + AcceleratorForwardOffset = 0; + GyroscopePanOffset = 0; + sCountOfUndisturbedFifoChunks = 0; + resetMPU6050FifoAndCarData(); // flush FIFO content and reset car data. Speed.Long and TurnAngle.Long serve as temporarily accumulator for offset value +} + +void IMUCarData::resetOffsetDataAndWait() { + resetOffsetData(); + while (AcceleratorForwardOffset == 0) { + readCarDataFromMPU6050Fifo(); + delay(DELAY_TO_NEXT_IMU_DATA_MILLIS); + } +} + +void IMUCarData::resetMPU6050FifoAndCarData() { + MPU6050WriteByte(MPU6050_RA_USER_CTRL, _BV(MPU6050_USERCTRL_FIFO_RESET_BIT)); // Reset FIFO + MPU6050WriteByte(MPU6050_RA_USER_CTRL, _BV(MPU6050_USERCTRL_FIFO_EN_BIT)); // enable FIFO + resetCarData(); // values might be invalid +} + +void IMUCarData::initMPU6050FifoForCarData() { + initMPU6050(); + MPU6050WriteByte(MPU6050_RA_FIFO_EN, _BV(MPU6050_ACCEL_FIFO_EN_BIT) | _BV(MPU6050_ZG_FIFO_EN_BIT)); // FIFO: all Accel axes + Gyro Z enabled + resetOffsetData(); +} + +/* + * I2C fast mode is supported by MPU6050 + */ +void IMUCarData::initWire() { + Wire.begin(); + Wire.setClock(400000); + Wire.setWireTimeout(5000); // Sets timeout to 5 ms. default is 25 ms. +} + +void IMUCarData::initMPU6050() { + initWire(); + MPU6050WriteByte(MPU6050_RA_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO); // use recommended gyro reference: PLL with Z axis gyroscope reference +#if SAMPLE_RATE == 1000 + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x00); // // Set sample rate to 1 kHz, divider is minimum (1) + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_188); // ext input disabled, DLPF enabled: accel 184Hz gyro 188Hz @1kHz +#elif SAMPLE_RATE == 500 + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x01); // // Set sample rate to 1/2 kHz, divider is 2 + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_42); // ext input disabled, DLPF enabled: ~50 Hz Sample freq = 1kHz +#elif SAMPLE_RATE == 250 + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x03); // // Set sample rate to 1/4 kHz, divider is 4 + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_20); // ext input disabled, DLPF enabled: ~20 Hz Sample freq = 1kHz +#else + MPU6050WriteByte(MPU6050_RA_SMPLRT_DIV, 0x07); // // Set sample rate to 1/8 kHz, divider is 8 + MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_10); // ext input disabled, DLPF enabled: ~10 Hz Sample freq = 1kHz +#endif + +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_10); // ext input disabled, DLPF enabled: ~10 Hz Sample freq = 1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_20); // ext input disabled, DLPF enabled: ~20 Hz Sample freq = 1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_42); // ext input disabled, DLPF enabled: ~50 Hz Sample freq = 1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_98); // ext input disabled, DLPF enabled: accel 184Hz gyro 188Hz @1kHz +// MPU6050WriteByte(MPU6050_RA_CONFIG, MPU6050_DLPF_BW_188); // ext input disabled, DLPF enabled: accel 184Hz gyro 188Hz @1kHz + +// the next is default and never changed by us +// MPU6050WriteByte(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACCEL_FS_2 << (MPU6050_ACONFIG_AFS_SEL_BIT - MPU6050_ACONFIG_AFS_SEL_LENGTH + 1)); // range +/- 2g +// MPU6050WriteByte(MPU6050_RA_GYRO_CONFIG, MPU6050_GYRO_FS_250 << (MPU6050_GCONFIG_FS_SEL_BIT - MPU6050_GCONFIG_FS_SEL_LENGTH + 1)); // range +/- 250 - default +} + +void IMUCarData::MPU6050WriteByte(uint8_t aRegisterNumber, uint8_t aData) { + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(aRegisterNumber); + Wire.write(aData); + Wire.endTransmission(); +} + +/* + * Read high byte first + */ +uint16_t IMUCarData::MPU6050ReadWordSwapped(uint8_t aRegisterNumber) { + Wire.beginTransmission(MPU6050_DEFAULT_ADDRESS); + Wire.write(aRegisterNumber); + Wire.endTransmission(false); + Wire.requestFrom((uint8_t) MPU6050_DEFAULT_ADDRESS, (uint8_t) 2, (uint8_t) true); + WordUnion tWord; + tWord.UByte.HighByte = Wire.read(); + tWord.UByte.LowByte = Wire.read(); + return tWord.UWord; +} diff --git a/src/IMUCarData.h b/src/IMUCarData.h new file mode 100644 index 0000000..1db04c7 --- /dev/null +++ b/src/IMUCarData.h @@ -0,0 +1,158 @@ +/* + * CarIMUData.h + * + * Functions for getting IMU data from MPU6050 for car control. + * + * Created on: 19.11.2020 + * Copyright (C) 2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. + * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#ifndef CAR_IMU_DATA_H_ +#define CAR_IMU_DATA_H_ + +#include +#include "MPU6050Defines.h" +#include "LongUnion.h" + +/* + * Activate this if the y axis of the GY-521 MPU6050 breakout board points forward / backward, i.e. connectors are at the right. + */ +// #define USE_ACCELERATOR_Y_FOR_SPEED +/* + * Activate this if the axis of the GY-521 MPU6050 breakout board points backwards. + */ +// #define USE_NEGATIVE_ACCELERATION_FOR_SPEED +// #define DISABLE_AUTO_OFFSET // default is enabled +// +#define NUMBER_OF_OFFSET_CALIBRATION_SAMPLES 512 +#define ACCEL_MOVE_THRESHOLD 128 +#define GYRO_MOVE_THRESHOLD 64 + +#ifndef SAMPLE_RATE +#define SAMPLE_RATE 1000 +//#define SAMPLE_RATE 500 +//#define SAMPLE_RATE 250 +//#define SAMPLE_RATE 125 +#endif +#if SAMPLE_RATE == 1000 +#define RATE_SHIFT 0 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 1 +#elif SAMPLE_RATE == 500 +#define RATE_SHIFT 1 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 2 +#elif SAMPLE_RATE == 250 +#define RATE_SHIFT 2 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 4 +#elif SAMPLE_RATE == 125 +#define RATE_SHIFT 3 +#define DELAY_TO_NEXT_IMU_DATA_MILLIS 8 +#else +#error SAMPLE_RATE must be 1000, 500, 250 or 125 +#endif + +#define ACCEL_RAW_TO_G_FOR_2G_RANGE (4.0/65536.0) +#define GYRO_RAW_TO_DEGREE_PER_SECOND_FOR_250DPS_RANGE (500.0/65536.0) // 0.00762939 or 1/131.072 + +class IMUCarData { +public: + +// IMUCarData(); + + void initWire(); // Is called by initMPU6050() + void initMPU6050(); + void initMPU6050FifoForCarData(); + unsigned int getMPU6050SampleRate(); // just returns SAMPLE_RATE + + void resetCarData(); + void resetMPU6050FifoAndCarData(); + void reset(); + void resetOffsetData(); + void resetOffsetDataAndWait(); + + void readCarDataFromMPU6050(); + bool readCarDataFromMPU6050Fifo(); + + void delayAndReadIMUCarDataData(unsigned long aMillisDelay); + + void printIMUCarDataCaption(Print *aSerial); + bool printIMUCarDataDataPeriodically(Print *aSerial, uint16_t aPeriodMillis); + void printIMUCarData(Print *aSerial); + + int8_t getAcceleratorForward15MilliG(); + int8_t getAcceleratorForward4MilliG(); + int8_t getAcceleratorForwardLowPass8(); + int8_t getAcceleratorForwardLowPass4(); + int getSpeedCmPerSecond(); + int getSpeedFastWithHigherResolution(); // faster and shorter call + int getDistanceCm(); + int getDistanceMillimeter(); + + int getGyroscopePan2DegreePerSecond(); + int getTurnAngleHalfDegree(); + int getTurnAngleDegree(); + + void doAutoOffset(); + void calculateSpeedAndTurnOffsets(); + + void printSpeedAndTurnOffsets(Print *aSerial); + + uint8_t MPU6050ReadByte(uint8_t aRegisterNumber); + uint16_t MPU6050ReadWordSwapped(uint8_t aRegisterNumber); + void MPU6050WriteByte(uint8_t aRegisterNumber, uint8_t aData); + + /* + * AcceleratorForwardOffset == 0 means, offsets are uninitialized and values are wrong + */ + bool OffsetsHaveChanged; + int16_t AcceleratorForwardOffset; // At 1kHz, offset 1 gives speed 4 per second / 1 per 4 second for 0.625 mm/s / 1cm/s resolution + WordUnion AcceleratorForward; // Average of every bulk read from the fifo. 16384 LSB per g => 61 ug per LSB, ~16 mg per 256 LSB + LongUnion AcceleratorForwardLowPass4; + LongUnion AcceleratorForwardLowPass8; + /* + * Speed is the sum of AcceleratorForward. + * At 1 kHz and 1 g (~10 m/s^2): Per sample (1/1000 second) we have 16384 (0x4000) and 10/1000 m/s (1 cm/s). + */ + LongUnion Speed; // shift 14 -> 1 (0.981) cm/s , 12 -> 2.5 mm/s, 8 -> 0.15328 mm/s + /* + * Distance is the sum of Speed >> 8. + * At 1 kHz: with 1 cm/s (shift (14-8)) we have 1 cm (or shift (24-8)) after 1 second (1000 additions) + */ + LongUnion Distance; + + int16_t GyroscopePanOffset; + WordUnion GyroscopePan; // Values without offset, +/-250 | 500 degree per second at 16 bit full range -> ~2 dps per 256 LSB + /* + * 1000 samples / second + * => 17 bit LSB per degree at +/-250 dps range + * The upper word has a resolution of 1/2 degree at 1000 samples per second + */ + LongUnion TurnAngle; + uint32_t LastFifoCheckMillis; + + /* + * Variables for AutoOffset + */ + int16_t sCountOfUndisturbedFifoChunks = 0; // signed, since it is used in formulas with other signed values + int32_t sSpeedSnapshot; + int32_t sTurnSnapshot; +}; + +#endif /* CAR_IMU_DATA_H_ */ + +#pragma once diff --git a/src/LightweightServo.cpp b/src/LightweightServo.cpp new file mode 100644 index 0000000..cd0e889 --- /dev/null +++ b/src/LightweightServo.cpp @@ -0,0 +1,220 @@ +/* + * LightweightServo.cpp + * + * Lightweight Servo implementation only for pin 9 and 10 using only timer1 hardware and no interrupts or other overhead. + * Provides auto initialization. + * 300 bytes code size / 4 bytes RAM including auto initialization compared to 700 / 48 bytes for Arduino Servo library. + * 8 bytes for each call to setLightweightServoPulse... + * + * Copyright (C) 2019 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of ServoEasing https://github.com/ArminJo/ServoEasing. + * + * ServoEasing is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) +#include "LightweightServo.h" + +/* + * Variables to enable adjustment for different servo types + * 544 and 2400 are values compatible with standard arduino values + * 4 bytes RAM compared to 48 bytes for standard Arduino library + */ +int sMicrosecondsForServo0Degree = 544; +int sMicrosecondsForServo180Degree = 2400; + +/* + * Use 16 bit timer1 for generating 2 servo signals entirely by hardware without any interrupts. + * Use FastPWM mode and generate pulse at start of the 20 ms period + * The 2 servo signals are tied to pin 9 and 10 of an 328. + * Attention - both pins are set to OUTPUT here! + * 32 bytes code size + */ +void initLightweightServoPin9And10() { + /* + * Periods below 20 ms gives problems with long signals i.e. the positioning is not possible + */ + DDRB |= _BV(DDB1) | _BV(DDB2); // set pins OC1A = PortB1 -> PIN 9 and OC1B = PortB2 -> PIN 10 to output direction + TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11); // FastPWM Mode mode TOP (20 ms) determined by ICR1 - non-inverting Compare Output mode OC1A+OC1B + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM mode mode bits WGM13 + WGM12 + ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms + // do not set counter here, since with counter = 0 (default) no output signal is generated. +} + +/* + * Use 16 bit timer1 for generating 2 servo signals entirely by hardware without any interrupts. + * Use FastPWM mode and generate pulse at start of the 20 ms period + * The 2 servo signals are tied to pin 9 and 10 of an ATMega328. + * Attention - the selected pin is set to OUTPUT here! + * 54 bytes code size + */ +void initLightweightServoPin9_10(bool aUsePin9, bool aUsePin10) { + + uint8_t tNewTCCR1A = TCCR1A & (_BV(COM1A1) | _BV(COM1B1)); // keep existing COM1A1 and COM1B1 settings + tNewTCCR1A |= _BV(WGM11); // FastPWM Mode mode TOP (20 ms) determined by ICR1 + + if (aUsePin9) { + DDRB |= _BV(DDB1); // set OC1A = PortB1 -> PIN 9 to output direction + tNewTCCR1A |= _BV(COM1A1); // non-inverting Compare Output mode OC1A + OCR1A = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. + } + if (aUsePin10) { + DDRB |= _BV(DDB2); // set OC1B = PortB2 -> PIN 10 to output direction + tNewTCCR1A |= _BV(COM1B1); // non-inverting Compare Output mode OC1B + OCR1B = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. + } + TCCR1A = tNewTCCR1A; + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 + ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms +} + +/* + * Disables Pin 10! + */ +void initLightweightServoPin9() { + DDRB |= _BV(DDB1); // set OC1A = PortB1 -> PIN 9 to output direction + TCCR1A = _BV(WGM11) | _BV(COM1A1); // FastPWM Mode mode TOP (20 ms) determined by ICR1, non-inverting Compare Output mode OC1A + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 + ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms + OCR1A = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. +} +/* + * Disables Pin 9! + */ +void initLightweightServoPin10() { + DDRB |= _BV(DDB2); // set OC1B = PortB2 -> PIN 10 to output direction + TCCR1A = _BV(WGM11) | _BV(COM1B1); // FastPWM Mode mode TOP (20 ms) determined by ICR1, non-inverting Compare Output mode OC1B + TCCR1B = _BV(WGM13) | _BV(WGM12) | _BV(CS11); // set prescaler to 8, FastPWM Mode mode bits WGM13 + WGM12 + ICR1 = ISR1_COUNT_FOR_20_MILLIS; // set period to 20 ms + OCR1B = 0xFFFF; // Set counter > ICR1 here, to avoid output signal generation. +} + +void deinitLightweightServoPin9_10(bool aUsePin9, bool aUsePin10) { + if (aUsePin9) { + DDRB &= ~(_BV(DDB1)); // set OC1A = PortB1 -> PIN 9 to input direction + TCCR1A &= ~(_BV(COM1A1)); // disable non-inverting Compare Output mode OC1A + } + if (aUsePin10) { + DDRB &= ~(_BV(DDB2)); // set OC1B = PortB2 -> PIN 10 to input direction + TCCR1A &= ~(_BV(COM1B1)); // disable non-inverting Compare Output mode OC1B + } +} + +/* + * If value is below 180 then assume degree, otherwise assume microseconds + * If aUpdateFast then enable starting a new output pulse if more than 5 ms since last one, some servo might react faster in this mode. + * If aUsePin9 is false, then Pin10 is used + * 236 / 186(without auto init) bytes code size + */ +int writeLightweightServo(int aDegree, bool aUsePin9, bool aUpdateFast) { + if (aDegree <= 180) { + aDegree = DegreeToMicrosecondsLightweightServo(aDegree); + } + writeMicrosecondsLightweightServo(aDegree, aUsePin9, aUpdateFast); + return aDegree; +} + +void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aUpdateFast) { +#ifndef DISABLE_SERVO_TIMER_AUTO_INITIALIZE + // auto initialize + if ((TCCR1B != (_BV(WGM13) | _BV(WGM12) | _BV(CS11))) || (aUsePin9 && ((TCCR1A & ~_BV(COM1B1)) != (_BV(COM1A1) | _BV(WGM11)))) + || (!aUsePin9 && ((TCCR1A & ~_BV(COM1A1)) != (_BV(COM1B1) | _BV(WGM11))))) { + initLightweightServoPin9_10(aUsePin9, !aUsePin9); + } +#endif + // since the resolution is 1/2 of microsecond + aMicroseconds *= 2; + if (aUpdateFast) { + uint16_t tTimerCount = TCNT1; + if (tTimerCount > 5000) { + // more than 2.5 ms since last pulse -> start a new one + TCNT1 = ICR1 - 1; + } + } + if (aUsePin9) { + OCR1A = aMicroseconds; + } else { + OCR1B = aMicroseconds; + } +} + +/* + * Sets the period of the servo pulses. Reasonable values are 2500 to 20000 microseconds. + * No parameter checking is done here! + */ +void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds) { + ICR1 = aRefreshPeriodMicroseconds * 2; +} +/* + * Set the mapping pulse width values for 0 and 180 degree + */ +void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int aMicrosecondsForServo180Degree) { + sMicrosecondsForServo0Degree = aMicrosecondsForServo0Degree; + sMicrosecondsForServo180Degree = aMicrosecondsForServo180Degree; +} + +/* + * Pin 9 / Channel A. If value is below 180 then assume degree, otherwise assume microseconds + */ +void write9(int aDegree, bool aUpdateFast) { + writeLightweightServo(aDegree, true, aUpdateFast); +} + +void writeMicroseconds9(int aMicroseconds, bool aUpdateFast) { + writeMicrosecondsLightweightServo(aMicroseconds, true, aUpdateFast); +} + +/* + * Without auto initialize! + */ +void writeMicroseconds9Direct(int aMicroseconds) { + OCR1A = aMicroseconds * 2; +} + +/* + * Pin 10 / Channel B + */ +void write10(int aDegree, bool aUpdateFast) { + writeLightweightServo(aDegree, false, aUpdateFast); +} + +void writeMicroseconds10(int aMicroseconds, bool aUpdateFast) { + writeMicrosecondsLightweightServo(aMicroseconds, false, aUpdateFast); +} + +/* + * Without auto initialize! + */ +void writeMicroseconds10Direct(int aMicroseconds) { + OCR1B = aMicroseconds * 2; +} + +/* + * Conversion functions + */ +int DegreeToMicrosecondsLightweightServo(int aDegree) { + return (map(aDegree, 0, 180, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree)); +} + +int MicrosecondsToDegreeLightweightServo(int aMicroseconds) { + return map(aMicroseconds, sMicrosecondsForServo0Degree, sMicrosecondsForServo180Degree, 0, 180); +} + +#endif + diff --git a/src/LightweightServo.h b/src/LightweightServo.h new file mode 100644 index 0000000..9084063 --- /dev/null +++ b/src/LightweightServo.h @@ -0,0 +1,78 @@ +/* + * LightweightServo.h + * + * Copyright (C) 2019-2020 Armin Joachimsmeyer + * armin.joachimsmeyer@gmail.com + * + * This file is part of ServoEasing https://github.com/ArminJo/ServoEasing. + * + * ServoEasing is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef LIGHTWEIGHT_SERVO_H_ +#define LIGHTWEIGHT_SERVO_H_ + +#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__) + +#define VERSION_LIGHTWEIGHT_SERVO "1.1.0" +#define VERSION_LIGHTWEIGHT_SERVO_MAJOR 1 +#define VERSION_LIGHTWEIGHT_SERVO_MINOR 1 + +#include + +/* + * Commenting out this saves 70 bytes flash memory. You must then use the init function initLightweightServoPin9And10() manually. + */ +//#define DISABLE_SERVO_TIMER_AUTO_INITIALIZE +#define ISR1_COUNT_FOR_20_MILLIS 40000 // you can modify this if you have servos which accept a higher rate + +/* + * Lightweight servo library + * Uses timer1 and Pin 9 + 10 as output + */ +void initLightweightServoPin9And10(); +void initLightweightServoPin9(); // Disables Pin 10! +void initLightweightServoPin10(); // Disables Pin 9! +void initLightweightServoPin9_10(bool aUsePin9, bool aUsePin10); +void deinitLightweightServoPin9_10(bool aUsePin9, bool aUsePin10); + +void setLightweightServoPulseMicrosFor0And180Degree(int aMicrosecondsForServo0Degree, int a180DegreeValue); +void setLightweightServoRefreshRate(unsigned int aRefreshPeriodMicroseconds); + +int writeLightweightServo(int aDegree, bool aUsePin9, bool aUpdateFast = false); +void writeMicrosecondsLightweightServo(int aMicroseconds, bool aUsePin9, bool aUpdateFast = false); + +void write9(int aDegree, bool aUpdateFast = false); // setLightweightServoPulsePin9 Channel A +void writeMicroseconds9(int aMicroseconds, bool aUpdateFast = false); +void writeMicroseconds9Direct(int aMicroseconds); + +void write10(int aDegree, bool aUpdateFast = false); // setLightweightServoPulsePin10 Channel B +void writeMicroseconds10(int aMicroseconds, bool aUpdateFast = false); +void writeMicroseconds10Direct(int aMicroseconds); + +// convenience functions +int DegreeToMicrosecondsLightweightServo(int aDegree); +int MicrosecondsToDegreeLightweightServo(int aMicroseconds); + +#endif // AVR_ATmega328 + +/* + * Version 1.1.0 - 11/2020 + * - Improved API. + */ + +#endif // LIGHTWEIGHT_SERVO_H_ + +#pragma once diff --git a/src/LongUnion.h b/src/LongUnion.h new file mode 100644 index 0000000..b0b2c2d --- /dev/null +++ b/src/LongUnion.h @@ -0,0 +1,94 @@ +/* + * LongUnion.h + * + * Copyright (C) 2020 Armin Joachimsmeyer + * Email: armin.joachimsmeyer@gmail.com + * + * This file is part of Arduino-Utils https://github.com/ArminJo/Arduino-Utils. + * + * Arduino-Utils is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#ifndef LONG_UNION_H +#define LONG_UNION_H + +#include +#include + +/** + * Union to specify parts / manifestations of a 16 bit Word without casts and shifts. + * It also supports the compiler generating small code. + */ +union WordUnion { + struct { + uint8_t LowByte; + uint8_t HighByte; + } UByte; + struct { + int8_t LowByte; + int8_t HighByte; + } Byte; + uint8_t UBytes[2]; + int8_t Bytes[2]; + uint16_t UWord; + int16_t Word; + uint8_t * BytePointer; +}; + +/** + * Union to specify parts / manifestations of a 32 bit Long without casts and shifts. + * It also supports the compiler generating small code. + */ +union LongUnion { + struct { + uint8_t LowByte; + uint8_t MidLowByte; + uint8_t MidHighByte; + uint8_t HighByte; + } UByte; + struct { + int8_t LowByte; + int8_t MidLowByte; + int8_t MidHighByte; + int8_t HighByte; + } Byte; + struct { + uint8_t LowByte; + WordUnion MidWord; + uint8_t HighByte; + } ByteWord; + struct { + int16_t LowWord; + int16_t HighWord; + } Word; + struct { + WordUnion LowWord; + WordUnion HighWord; + } WordUnion; + struct { + uint16_t LowWord; + uint16_t HighWord; + } UWord; + uint8_t UBytes[4]; + int8_t Bytes[4]; + uint16_t UWords[2]; + int16_t Words[2]; + uint32_t ULong; + int32_t Long; +}; + +#endif // LONG_UNION_H + +#pragma once diff --git a/src/MPU6050Defines.h b/src/MPU6050Defines.h new file mode 100644 index 0000000..221d94f --- /dev/null +++ b/src/MPU6050Defines.h @@ -0,0 +1,428 @@ +/* + * MPU6050Defines.h + * Contains only the defines for MPU6050. + * Copied from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 + */ + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_H_ +#define _MPU6050_H_ + +//#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 + +// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 +// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s + +#ifdef __AVR__ +#include +#else +//#define PROGMEM /* empty */ +//#define pgm_read_byte(x) (*(x)) +//#define pgm_read_word(x) (*(x)) +//#define pgm_read_float(x) (*(x)) +//#define PSTR(STR) STR +#endif + + +#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board +#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW + +#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD +#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN +#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN +#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN +#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU6050_RA_XA_OFFS_L_TC 0x07 +#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS +#define MPU6050_RA_YA_OFFS_L_TC 0x09 +#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS +#define MPU6050_RA_ZA_OFFS_L_TC 0x0B +#define MPU6050_RA_SELF_TEST_X 0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Y 0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_Z 0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0] +#define MPU6050_RA_SELF_TEST_A 0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0] +#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR +#define MPU6050_RA_XG_OFFS_USRL 0x14 +#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR +#define MPU6050_RA_YG_OFFS_USRL 0x16 +#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR +#define MPU6050_RA_ZG_OFFS_USRL 0x18 +#define MPU6050_RA_SMPLRT_DIV 0x19 +#define MPU6050_RA_CONFIG 0x1A +#define MPU6050_RA_GYRO_CONFIG 0x1B +#define MPU6050_RA_ACCEL_CONFIG 0x1C +#define MPU6050_RA_FF_THR 0x1D +#define MPU6050_RA_FF_DUR 0x1E +#define MPU6050_RA_MOT_THR 0x1F +#define MPU6050_RA_MOT_DUR 0x20 +#define MPU6050_RA_ZRMOT_THR 0x21 +#define MPU6050_RA_ZRMOT_DUR 0x22 +#define MPU6050_RA_FIFO_EN 0x23 +#define MPU6050_RA_I2C_MST_CTRL 0x24 +#define MPU6050_RA_I2C_SLV0_ADDR 0x25 +#define MPU6050_RA_I2C_SLV0_REG 0x26 +#define MPU6050_RA_I2C_SLV0_CTRL 0x27 +#define MPU6050_RA_I2C_SLV1_ADDR 0x28 +#define MPU6050_RA_I2C_SLV1_REG 0x29 +#define MPU6050_RA_I2C_SLV1_CTRL 0x2A +#define MPU6050_RA_I2C_SLV2_ADDR 0x2B +#define MPU6050_RA_I2C_SLV2_REG 0x2C +#define MPU6050_RA_I2C_SLV2_CTRL 0x2D +#define MPU6050_RA_I2C_SLV3_ADDR 0x2E +#define MPU6050_RA_I2C_SLV3_REG 0x2F +#define MPU6050_RA_I2C_SLV3_CTRL 0x30 +#define MPU6050_RA_I2C_SLV4_ADDR 0x31 +#define MPU6050_RA_I2C_SLV4_REG 0x32 +#define MPU6050_RA_I2C_SLV4_DO 0x33 +#define MPU6050_RA_I2C_SLV4_CTRL 0x34 +#define MPU6050_RA_I2C_SLV4_DI 0x35 +#define MPU6050_RA_I2C_MST_STATUS 0x36 +#define MPU6050_RA_INT_PIN_CFG 0x37 +#define MPU6050_RA_INT_ENABLE 0x38 +#define MPU6050_RA_DMP_INT_STATUS 0x39 +#define MPU6050_RA_INT_STATUS 0x3A +#define MPU6050_RA_ACCEL_XOUT_H 0x3B +#define MPU6050_RA_ACCEL_XOUT_L 0x3C +#define MPU6050_RA_ACCEL_YOUT_H 0x3D +#define MPU6050_RA_ACCEL_YOUT_L 0x3E +#define MPU6050_RA_ACCEL_ZOUT_H 0x3F +#define MPU6050_RA_ACCEL_ZOUT_L 0x40 +#define MPU6050_RA_TEMP_OUT_H 0x41 +#define MPU6050_RA_TEMP_OUT_L 0x42 +#define MPU6050_RA_GYRO_XOUT_H 0x43 +#define MPU6050_RA_GYRO_XOUT_L 0x44 +#define MPU6050_RA_GYRO_YOUT_H 0x45 +#define MPU6050_RA_GYRO_YOUT_L 0x46 +#define MPU6050_RA_GYRO_ZOUT_H 0x47 +#define MPU6050_RA_GYRO_ZOUT_L 0x48 +#define MPU6050_RA_EXT_SENS_DATA_00 0x49 +#define MPU6050_RA_EXT_SENS_DATA_01 0x4A +#define MPU6050_RA_EXT_SENS_DATA_02 0x4B +#define MPU6050_RA_EXT_SENS_DATA_03 0x4C +#define MPU6050_RA_EXT_SENS_DATA_04 0x4D +#define MPU6050_RA_EXT_SENS_DATA_05 0x4E +#define MPU6050_RA_EXT_SENS_DATA_06 0x4F +#define MPU6050_RA_EXT_SENS_DATA_07 0x50 +#define MPU6050_RA_EXT_SENS_DATA_08 0x51 +#define MPU6050_RA_EXT_SENS_DATA_09 0x52 +#define MPU6050_RA_EXT_SENS_DATA_10 0x53 +#define MPU6050_RA_EXT_SENS_DATA_11 0x54 +#define MPU6050_RA_EXT_SENS_DATA_12 0x55 +#define MPU6050_RA_EXT_SENS_DATA_13 0x56 +#define MPU6050_RA_EXT_SENS_DATA_14 0x57 +#define MPU6050_RA_EXT_SENS_DATA_15 0x58 +#define MPU6050_RA_EXT_SENS_DATA_16 0x59 +#define MPU6050_RA_EXT_SENS_DATA_17 0x5A +#define MPU6050_RA_EXT_SENS_DATA_18 0x5B +#define MPU6050_RA_EXT_SENS_DATA_19 0x5C +#define MPU6050_RA_EXT_SENS_DATA_20 0x5D +#define MPU6050_RA_EXT_SENS_DATA_21 0x5E +#define MPU6050_RA_EXT_SENS_DATA_22 0x5F +#define MPU6050_RA_EXT_SENS_DATA_23 0x60 +#define MPU6050_RA_MOT_DETECT_STATUS 0x61 +#define MPU6050_RA_I2C_SLV0_DO 0x63 +#define MPU6050_RA_I2C_SLV1_DO 0x64 +#define MPU6050_RA_I2C_SLV2_DO 0x65 +#define MPU6050_RA_I2C_SLV3_DO 0x66 +#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67 +#define MPU6050_RA_SIGNAL_PATH_RESET 0x68 +#define MPU6050_RA_MOT_DETECT_CTRL 0x69 +#define MPU6050_RA_USER_CTRL 0x6A +#define MPU6050_RA_PWR_MGMT_1 0x6B +#define MPU6050_RA_PWR_MGMT_2 0x6C +#define MPU6050_RA_BANK_SEL 0x6D +#define MPU6050_RA_MEM_START_ADDR 0x6E +#define MPU6050_RA_MEM_R_W 0x6F +#define MPU6050_RA_DMP_CFG_1 0x70 +#define MPU6050_RA_DMP_CFG_2 0x71 +#define MPU6050_RA_FIFO_COUNTH 0x72 +#define MPU6050_RA_FIFO_COUNTL 0x73 +#define MPU6050_RA_FIFO_R_W 0x74 +#define MPU6050_RA_WHO_AM_I 0x75 + +#define MPU6050_SELF_TEST_XA_1_BIT 0x07 +#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_XA_2_BIT 0x05 +#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_YA_1_BIT 0x07 +#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_YA_2_BIT 0x03 +#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02 +#define MPU6050_SELF_TEST_ZA_1_BIT 0x07 +#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03 +#define MPU6050_SELF_TEST_ZA_2_BIT 0x01 +#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02 + +#define MPU6050_SELF_TEST_XG_1_BIT 0x04 +#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_YG_1_BIT 0x04 +#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05 +#define MPU6050_SELF_TEST_ZG_1_BIT 0x04 +#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05 + +#define MPU6050_TC_PWR_MODE_BIT 7 +#define MPU6050_TC_OFFSET_BIT 6 +#define MPU6050_TC_OFFSET_LENGTH 6 +#define MPU6050_TC_OTP_BNK_VLD_BIT 0 + +#define MPU6050_VDDIO_LEVEL_VLOGIC 0 +#define MPU6050_VDDIO_LEVEL_VDD 1 + +#define MPU6050_CFG_EXT_SYNC_SET_BIT 5 +#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3 +#define MPU6050_CFG_DLPF_CFG_BIT 2 +#define MPU6050_CFG_DLPF_CFG_LENGTH 3 + +#define MPU6050_EXT_SYNC_DISABLED 0x0 +#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1 +#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2 +#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3 +#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4 +#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5 +#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6 +#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7 + +#define MPU6050_DLPF_BW_256 0x00 +#define MPU6050_DLPF_BW_188 0x01 +#define MPU6050_DLPF_BW_98 0x02 +#define MPU6050_DLPF_BW_42 0x03 +#define MPU6050_DLPF_BW_20 0x04 +#define MPU6050_DLPF_BW_10 0x05 +#define MPU6050_DLPF_BW_5 0x06 + +#define MPU6050_GCONFIG_FS_SEL_BIT 4 +#define MPU6050_GCONFIG_FS_SEL_LENGTH 2 + +#define MPU6050_GYRO_FS_250 0x00 +#define MPU6050_GYRO_FS_500 0x01 +#define MPU6050_GYRO_FS_1000 0x02 +#define MPU6050_GYRO_FS_2000 0x03 + +#define MPU6050_ACONFIG_XA_ST_BIT 7 +#define MPU6050_ACONFIG_YA_ST_BIT 6 +#define MPU6050_ACONFIG_ZA_ST_BIT 5 +#define MPU6050_ACONFIG_AFS_SEL_BIT 4 +#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2 +#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2 +#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3 + +#define MPU6050_ACCEL_FS_2 0x00 +#define MPU6050_ACCEL_FS_4 0x01 +#define MPU6050_ACCEL_FS_8 0x02 +#define MPU6050_ACCEL_FS_16 0x03 + +#define MPU6050_DHPF_RESET 0x00 +#define MPU6050_DHPF_5 0x01 +#define MPU6050_DHPF_2P5 0x02 +#define MPU6050_DHPF_1P25 0x03 +#define MPU6050_DHPF_0P63 0x04 +#define MPU6050_DHPF_HOLD 0x07 + +#define MPU6050_TEMP_FIFO_EN_BIT 7 +#define MPU6050_XG_FIFO_EN_BIT 6 +#define MPU6050_YG_FIFO_EN_BIT 5 +#define MPU6050_ZG_FIFO_EN_BIT 4 +#define MPU6050_ACCEL_FIFO_EN_BIT 3 +#define MPU6050_SLV2_FIFO_EN_BIT 2 +#define MPU6050_SLV1_FIFO_EN_BIT 1 +#define MPU6050_SLV0_FIFO_EN_BIT 0 + +#define MPU6050_MULT_MST_EN_BIT 7 +#define MPU6050_WAIT_FOR_ES_BIT 6 +#define MPU6050_SLV_3_FIFO_EN_BIT 5 +#define MPU6050_I2C_MST_P_NSR_BIT 4 +#define MPU6050_I2C_MST_CLK_BIT 3 +#define MPU6050_I2C_MST_CLK_LENGTH 4 + +#define MPU6050_CLOCK_DIV_348 0x0 +#define MPU6050_CLOCK_DIV_333 0x1 +#define MPU6050_CLOCK_DIV_320 0x2 +#define MPU6050_CLOCK_DIV_308 0x3 +#define MPU6050_CLOCK_DIV_296 0x4 +#define MPU6050_CLOCK_DIV_286 0x5 +#define MPU6050_CLOCK_DIV_276 0x6 +#define MPU6050_CLOCK_DIV_267 0x7 +#define MPU6050_CLOCK_DIV_258 0x8 +#define MPU6050_CLOCK_DIV_500 0x9 +#define MPU6050_CLOCK_DIV_471 0xA +#define MPU6050_CLOCK_DIV_444 0xB +#define MPU6050_CLOCK_DIV_421 0xC +#define MPU6050_CLOCK_DIV_400 0xD +#define MPU6050_CLOCK_DIV_381 0xE +#define MPU6050_CLOCK_DIV_364 0xF + +#define MPU6050_I2C_SLV_RW_BIT 7 +#define MPU6050_I2C_SLV_ADDR_BIT 6 +#define MPU6050_I2C_SLV_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV_EN_BIT 7 +#define MPU6050_I2C_SLV_BYTE_SW_BIT 6 +#define MPU6050_I2C_SLV_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV_GRP_BIT 4 +#define MPU6050_I2C_SLV_LEN_BIT 3 +#define MPU6050_I2C_SLV_LEN_LENGTH 4 + +#define MPU6050_I2C_SLV4_RW_BIT 7 +#define MPU6050_I2C_SLV4_ADDR_BIT 6 +#define MPU6050_I2C_SLV4_ADDR_LENGTH 7 +#define MPU6050_I2C_SLV4_EN_BIT 7 +#define MPU6050_I2C_SLV4_INT_EN_BIT 6 +#define MPU6050_I2C_SLV4_REG_DIS_BIT 5 +#define MPU6050_I2C_SLV4_MST_DLY_BIT 4 +#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5 + +#define MPU6050_MST_PASS_THROUGH_BIT 7 +#define MPU6050_MST_I2C_SLV4_DONE_BIT 6 +#define MPU6050_MST_I2C_LOST_ARB_BIT 5 +#define MPU6050_MST_I2C_SLV4_NACK_BIT 4 +#define MPU6050_MST_I2C_SLV3_NACK_BIT 3 +#define MPU6050_MST_I2C_SLV2_NACK_BIT 2 +#define MPU6050_MST_I2C_SLV1_NACK_BIT 1 +#define MPU6050_MST_I2C_SLV0_NACK_BIT 0 + +#define MPU6050_INTCFG_INT_LEVEL_BIT 7 +#define MPU6050_INTCFG_INT_OPEN_BIT 6 +#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5 +#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4 +#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3 +#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2 +#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1 +#define MPU6050_INTCFG_CLKOUT_EN_BIT 0 + +#define MPU6050_INTMODE_ACTIVEHIGH 0x00 +#define MPU6050_INTMODE_ACTIVELOW 0x01 + +#define MPU6050_INTDRV_PUSHPULL 0x00 +#define MPU6050_INTDRV_OPENDRAIN 0x01 + +#define MPU6050_INTLATCH_50USPULSE 0x00 +#define MPU6050_INTLATCH_WAITCLEAR 0x01 + +#define MPU6050_INTCLEAR_STATUSREAD 0x00 +#define MPU6050_INTCLEAR_ANYREAD 0x01 + +#define MPU6050_INTERRUPT_FF_BIT 7 +#define MPU6050_INTERRUPT_MOT_BIT 6 +#define MPU6050_INTERRUPT_ZMOT_BIT 5 +#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4 +#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3 +#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2 +#define MPU6050_INTERRUPT_DMP_INT_BIT 1 +#define MPU6050_INTERRUPT_DATA_RDY_BIT 0 + +// TODO: figure out what these actually do +// UMPL source code is not very obivous +#define MPU6050_DMPINT_5_BIT 5 +#define MPU6050_DMPINT_4_BIT 4 +#define MPU6050_DMPINT_3_BIT 3 +#define MPU6050_DMPINT_2_BIT 2 +#define MPU6050_DMPINT_1_BIT 1 +#define MPU6050_DMPINT_0_BIT 0 + +#define MPU6050_MOTION_MOT_XNEG_BIT 7 +#define MPU6050_MOTION_MOT_XPOS_BIT 6 +#define MPU6050_MOTION_MOT_YNEG_BIT 5 +#define MPU6050_MOTION_MOT_YPOS_BIT 4 +#define MPU6050_MOTION_MOT_ZNEG_BIT 3 +#define MPU6050_MOTION_MOT_ZPOS_BIT 2 +#define MPU6050_MOTION_MOT_ZRMOT_BIT 0 + +#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7 +#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4 +#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3 +#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2 +#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1 +#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0 + +#define MPU6050_PATHRESET_GYRO_RESET_BIT 2 +#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1 +#define MPU6050_PATHRESET_TEMP_RESET_BIT 0 + +#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5 +#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2 +#define MPU6050_DETECT_FF_COUNT_BIT 3 +#define MPU6050_DETECT_FF_COUNT_LENGTH 2 +#define MPU6050_DETECT_MOT_COUNT_BIT 1 +#define MPU6050_DETECT_MOT_COUNT_LENGTH 2 + +#define MPU6050_DETECT_DECREMENT_RESET 0x0 +#define MPU6050_DETECT_DECREMENT_1 0x1 +#define MPU6050_DETECT_DECREMENT_2 0x2 +#define MPU6050_DETECT_DECREMENT_4 0x3 + +#define MPU6050_USERCTRL_DMP_EN_BIT 7 +#define MPU6050_USERCTRL_FIFO_EN_BIT 6 +#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5 +#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4 +#define MPU6050_USERCTRL_DMP_RESET_BIT 3 +#define MPU6050_USERCTRL_FIFO_RESET_BIT 2 +#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1 +#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0 + +#define MPU6050_PWR1_DEVICE_RESET_BIT 7 +#define MPU6050_PWR1_SLEEP_BIT 6 +#define MPU6050_PWR1_CYCLE_BIT 5 +#define MPU6050_PWR1_TEMP_DIS_BIT 3 +#define MPU6050_PWR1_CLKSEL_BIT 2 +#define MPU6050_PWR1_CLKSEL_LENGTH 3 + +#define MPU6050_CLOCK_INTERNAL 0x00 +#define MPU6050_CLOCK_PLL_XGYRO 0x01 +#define MPU6050_CLOCK_PLL_YGYRO 0x02 +#define MPU6050_CLOCK_PLL_ZGYRO 0x03 +#define MPU6050_CLOCK_PLL_EXT32K 0x04 +#define MPU6050_CLOCK_PLL_EXT19M 0x05 +#define MPU6050_CLOCK_KEEP_RESET 0x07 + +#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7 +#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2 +#define MPU6050_PWR2_STBY_XA_BIT 5 +#define MPU6050_PWR2_STBY_YA_BIT 4 +#define MPU6050_PWR2_STBY_ZA_BIT 3 +#define MPU6050_PWR2_STBY_XG_BIT 2 +#define MPU6050_PWR2_STBY_YG_BIT 1 +#define MPU6050_PWR2_STBY_ZG_BIT 0 + +#define MPU6050_WAKE_FREQ_1P25 0x0 +#define MPU6050_WAKE_FREQ_2P5 0x1 +#define MPU6050_WAKE_FREQ_5 0x2 +#define MPU6050_WAKE_FREQ_10 0x3 + +#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6 +#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5 +#define MPU6050_BANKSEL_MEM_SEL_BIT 4 +#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5 + +#define MPU6050_WHO_AM_I_BIT 6 +#define MPU6050_WHO_AM_I_LENGTH 6 + +#define MPU6050_DMP_MEMORY_BANKS 8 +#define MPU6050_DMP_MEMORY_BANK_SIZE 256 +#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 + +#endif /* _MPU6050_H_ */ diff --git a/src/PWMDcMotor.cpp b/src/PWMDcMotor.cpp index 1da8722..482c07a 100644 --- a/src/PWMDcMotor.cpp +++ b/src/PWMDcMotor.cpp @@ -4,15 +4,23 @@ * Low level motor control for Adafruit_MotorShield OR breakout board with TB6612 or L298 driver IC for two DC motors. * * Motor control has 2 parameters: - * 1. Speed / PWM which is ignored for BRAKE or RELEASE. This library also accepts signed speed (including the direction as sign). + * 1. SpeedPWM / PWM which is ignored for BRAKE or RELEASE. This library also accepts signed speed (including the direction as sign). * 2. Direction / MotorDriverMode. Can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance) * + * PWM period is 600 us for Adafruit Motor Shield V2 using PCA9685. + * PWM period is 1030 us for using AnalogWrite on pin 5 + 6. + * * Created on: 12.05.2019 - * Copyright (C) 2016-2020 Armin Joachimsmeyer + * Copyright (C) 2019-2020 Armin Joachimsmeyer * armin.joachimsmeyer@gmail.com * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -26,36 +34,43 @@ #include "PWMDcMotor.h" -bool PWMDcMotor::MotorValuesHaveChanged; // true if DefaultStopMode, StartSpeed, DriveSpeed or SpeedCompensation have changed - for printing -bool PWMDcMotor::SpeedOrMotorModeHasChanged; // - for printing +//#define TRACE +//#define DEBUG + +// Flags e.g. for display update control +#if defined(USE_MPU6050_IMU) || defined(USE_ENCODER_MOTOR_CONTROL) +volatile bool PWMDcMotor::SensorValuesHaveChanged; // true if encoder count and derived encoder speed, or one of the TMU data have changed +#endif +bool PWMDcMotor::MotorControlValuesHaveChanged; // true if DefaultStopMode, StartSpeedPWM, DriveSpeedPWM or SpeedPWMCompensation have changed +bool PWMDcMotor::MotorPWMHasChanged; // true if CurrentSpeedPWM has changed PWMDcMotor::PWMDcMotor() { // @suppress("Class members should be properly initialized") } #ifdef USE_ADAFRUIT_MOTOR_SHIELD # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD -void PWMDcMotor::I2CWriteByte(uint8_t aAddress, uint8_t aData) { - Wire.beginTransmission(0x60); +void PWMDcMotor::PCA9685WriteByte(uint8_t aAddress, uint8_t aData) { + Wire.beginTransmission(PCA9685_DEFAULT_ADDRESS); Wire.write(aAddress); Wire.write(aData); - Wire.endTransmission(); + Wire.endTransmission(true); } -void PWMDcMotor::I2CSetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff) { - Wire.beginTransmission(0x60); +void PWMDcMotor::PCA9685SetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff) { + Wire.beginTransmission(PCA9685_DEFAULT_ADDRESS); Wire.write((PCA9685_FIRST_PWM_REGISTER) + 4 * aPin); Wire.write(aOn); Wire.write(aOn >> 8); Wire.write(aOff); Wire.write(aOff >> 8); - Wire.endTransmission(); + Wire.endTransmission(true); } -void PWMDcMotor::I2CSetPin(uint8_t aPin, bool aSetToOn) { +void PWMDcMotor::PCA9685SetPin(uint8_t aPin, bool aSetToOn) { if (aSetToOn) { - I2CSetPWM(aPin, 4096, 0); + PCA9685SetPWM(aPin, 4096, 0); } else { - I2CSetPWM(aPin, 0, 0); + PCA9685SetPWM(aPin, 0, 0); } } @@ -68,8 +83,7 @@ Adafruit_MotorShield sAdafruitMotorShield = Adafruit_MotorShield(); * aMotorNumber from 1 to 2 * Currently motors 3 and 4 are not required/supported by own library for Adafruit Motor Shield */ -void PWMDcMotor::init(uint8_t aMotorNumber, bool aReadFromEeprom) { - +void PWMDcMotor::init(uint8_t aMotorNumber) { # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD if (aMotorNumber == 1) { // Set PCA9685 channel numbers for Adafruit Motor Shield @@ -83,40 +97,36 @@ void PWMDcMotor::init(uint8_t aMotorNumber, bool aReadFromEeprom) { } Wire.begin(); + Wire.setClock(400000); # if defined (ARDUINO_ARCH_AVR) // Other platforms do not have this new function Wire.setWireTimeout(5000); // Sets timeout to 5 ms. default is 25 ms. # endif +#ifdef TRACE + Serial.print(PWMPin); + Serial.print(F(" MotorNumber=")); + Serial.println(aMotorNumber); +#endif // Reset PCA9685 Wire.beginTransmission(PCA9685_GENERAL_CALL_ADDRESS); Wire.write(PCA9685_SOFTWARE_RESET); - Wire.endTransmission(); + Wire.endTransmission(true); // Set expander to 1600 HZ - I2CWriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_SLEEP)); // go to sleep - I2CWriteByte(PCA9685_PRESCALE_REGISTER, PCA9685_PRESCALER_FOR_1600_HZ); // set the prescaler + PCA9685WriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_SLEEP)); // go to sleep + PCA9685WriteByte(PCA9685_PRESCALE_REGISTER, PCA9685_PRESCALER_FOR_1600_HZ); // set the prescaler delay(2); // > 500 us before the restart bit according to datasheet - I2CWriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_RESTART) | _BV(PCA9685_MODE_1_AUTOINCREMENT)); // reset sleep and enable auto increment + PCA9685WriteByte(PCA9685_MODE1_REGISTER, _BV(PCA9685_MODE_1_RESTART) | _BV(PCA9685_MODE_1_AUTOINCREMENT)); // reset sleep and enable auto increment # else Adafruit_MotorShield_DcMotor = sAdafruitMotorShield.getMotor(aMotorNumber); sAdafruitMotorShield.begin(); # endif - if (!aReadFromEeprom) { - MotorValuesEepromStorageNumber = 0; - } else { - MotorValuesEepromStorageNumber = aMotorNumber; - } - // setDefaultsForFixedDistanceDriving() is called by readMotorValuesFromEeprom() if MotorValuesEepromStorageNumber == 0 - readMotorValuesFromEeprom(); + setDefaultsForFixedDistanceDriving(); stop(DEFAULT_STOP_MODE); } #else // USE_ADAFRUIT_MOTOR_SHIELD -/* - * @param aMotorNumber if 0 do not read from EEPROM, otherwise block number - */ -void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber) { - MotorValuesEepromStorageNumber = aMotorNumber; +void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin) { ForwardPin = aForwardPin; BackwardPin = aBackwardPin; PWMPin = aPWMPin; @@ -128,8 +138,6 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin // set defaults setDefaultsForFixedDistanceDriving(); - - readMotorValuesFromEeprom(); // reads values only if MotorValuesEepromStorageNumber / aMotorNumber is != 0 stop(DEFAULT_STOP_MODE); } @@ -141,25 +149,29 @@ void PWMDcMotor::init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin */ void PWMDcMotor::setMotorDriverMode(uint8_t aMotorDriverMode) { CurrentDirectionOrBrakeMode = aMotorDriverMode; // The only statement which changes CurrentDirectionOrBrakeMode + if (!(aMotorDriverMode & STOP_MODE_OR_MASK)) { + // set only directions, no brake modes + LastDirection = aMotorDriverMode; + } #ifdef USE_ADAFRUIT_MOTOR_SHIELD // until here DIRECTION_FORWARD is 0 back is 1, Adafruit library starts with 1 # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD switch (aMotorDriverMode) { case DIRECTION_FORWARD: - I2CSetPin(BackwardPin, LOW); // take low first to avoid 'break' - I2CSetPin(ForwardPin, HIGH); + PCA9685SetPin(BackwardPin, LOW); // take low first to avoid 'break' + PCA9685SetPin(ForwardPin, HIGH); break; case DIRECTION_BACKWARD: - I2CSetPin(ForwardPin, LOW); // take low first to avoid 'break' - I2CSetPin(BackwardPin, HIGH); + PCA9685SetPin(ForwardPin, LOW); // take low first to avoid 'break' + PCA9685SetPin(BackwardPin, HIGH); break; case MOTOR_BRAKE: - I2CSetPin(ForwardPin, HIGH); - I2CSetPin(BackwardPin, HIGH); + PCA9685SetPin(ForwardPin, HIGH); + PCA9685SetPin(BackwardPin, HIGH); break; case MOTOR_RELEASE: - I2CSetPin(ForwardPin, LOW); - I2CSetPin(BackwardPin, LOW); + PCA9685SetPin(ForwardPin, LOW); + PCA9685SetPin(BackwardPin, LOW); break; } # else @@ -189,24 +201,27 @@ void PWMDcMotor::setMotorDriverMode(uint8_t aMotorDriverMode) { } /* - * @return true if direction has changed and motor has stopped + * @return true if direction has changed AND motor was stopped */ bool PWMDcMotor::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { + aRequestedDirection &= DIRECTION_MASK; // since we are never called with "brake directions" but may be called with TURN_IN_PLACE direction bool tReturnValue = false; - uint8_t tRequestedDirection = aRequestedDirection & DIRECTION_MASK; - if (CurrentDirectionOrBrakeMode != tRequestedDirection) { - if (CurrentSpeed != 0) { -#ifdef DEBUG - Serial.print(F("Motor mode change to ")); - Serial.println(tRequestedDirection); -#endif + if (CurrentDirectionOrBrakeMode != aRequestedDirection) { + if (CurrentSpeedPWM != 0) { /* * Direction change requested but motor still running-> first stop motor */ stop(MOTOR_BRAKE); tReturnValue = true; } - setMotorDriverMode(tRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Change motor mode from ")); + Serial.print(CurrentDirectionOrBrakeMode); + Serial.print(F(" to ")); + Serial.println(aRequestedDirection); +#endif + setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode } return tReturnValue; } @@ -215,63 +230,88 @@ bool PWMDcMotor::checkAndHandleDirectionChange(uint8_t aRequestedDirection) { * @brief Control the DC Motor speed/throttle * @param speed The 8-bit PWM value, 0 is off, 255 is on forward -255 is on backward * First set driver mode, then set PWM + * PWM period is 600 us for Adafruit Motor Shield V2 using PCA9685. + * PWM period is 1030 us for using AnalogWrite on pin 5 + 6. */ -void PWMDcMotor::setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection) { - if (aSpeedRequested == 0) { +void PWMDcMotor::setSpeedPWM(uint8_t aSpeedPWMRequested, uint8_t aRequestedDirection) { + if (aSpeedPWMRequested == 0) { stop(DefaultStopMode); } else { checkAndHandleDirectionChange(aRequestedDirection); - if (CurrentSpeed != aSpeedRequested) { - CurrentSpeed = aSpeedRequested; // The only statement which sets CurrentSpeed to a value != 0 - SpeedOrMotorModeHasChanged = true; + if (CurrentSpeedPWM != aSpeedPWMRequested) { + CurrentSpeedPWM = aSpeedPWMRequested; // The only statement which sets CurrentSpeedPWM to a value != 0 +#ifdef TRACE + Serial.print(PWMPin); + Serial.print(F(" SpeedPWM=")); + Serial.println(CurrentSpeedPWM); +#endif + MotorPWMHasChanged = true; #ifdef USE_ADAFRUIT_MOTOR_SHIELD # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD - I2CSetPWM(PWMPin, 0, 16 * aSpeedRequested); + PCA9685SetPWM(PWMPin, 0, 16 * aSpeedPWMRequested); # else - Adafruit_MotorShield_DcMotor->setSpeed(aSpeedRequested); + Adafruit_MotorShield_DcMotor->setSpeedPWM(aSpeedPWMRequested); # endif #else - analogWrite(PWMPin, aSpeedRequested); + analogWrite(PWMPin, aSpeedPWMRequested); #endif } } } /* - * Subtracts SpeedCompensation from aRequestedSpeed before applying + * Keeps direction + */ +void PWMDcMotor::changeSpeedPWM(uint8_t aSpeedPWMRequested) { + if (!(CurrentDirectionOrBrakeMode & STOP_MODE_OR_MASK)) { + setSpeedPWM(aSpeedPWMRequested, CurrentDirectionOrBrakeMode); // output PWM value to motor + } +} + +/* + * Subtracts SpeedPWMCompensation from aRequestedSpeedPWM before applying */ -void PWMDcMotor::setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { +void PWMDcMotor::setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { // avoid underflow - uint8_t tCurrentSpeed; - if (aRequestedSpeed > SpeedCompensation) { - tCurrentSpeed = aRequestedSpeed - SpeedCompensation; + uint8_t tCurrentSpeedPWM; + if (aRequestedSpeedPWM > SpeedPWMCompensation) { + tCurrentSpeedPWM = aRequestedSpeedPWM - SpeedPWMCompensation; } else { - tCurrentSpeed = 0; + tCurrentSpeedPWM = 0; + } + setSpeedPWM(tCurrentSpeedPWM, aRequestedDirection); // output PWM value to motor +} + +/* + * Keeps direction + */ +void PWMDcMotor::changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM) { + if (!(CurrentDirectionOrBrakeMode & STOP_MODE_OR_MASK)) { + setSpeedPWMCompensated(aRequestedSpeedPWM, CurrentDirectionOrBrakeMode); } - setSpeed(tCurrentSpeed, aRequestedDirection); // output PWM value to motor } /* * Signed speed */ -void PWMDcMotor::setSpeed(int aSpeedRequested) { - if (aSpeedRequested < 0) { - aSpeedRequested = -aSpeedRequested; - setSpeed(aSpeedRequested, DIRECTION_BACKWARD); +void PWMDcMotor::setSpeedPWM(int aSpeedPWMRequested) { + if (aSpeedPWMRequested < 0) { + aSpeedPWMRequested = -aSpeedPWMRequested; + setSpeedPWM(aSpeedPWMRequested, DIRECTION_BACKWARD); } else { - setSpeed(aSpeedRequested, DIRECTION_FORWARD); + setSpeedPWM(aSpeedPWMRequested, DIRECTION_FORWARD); } } -void PWMDcMotor::setSpeedCompensated(int aRequestedSpeed) { +void PWMDcMotor::setSpeedPWMCompensated(int aRequestedSpeedPWM) { uint8_t tDirection; - if (aRequestedSpeed > 0) { + if (aRequestedSpeedPWM > 0) { tDirection = DIRECTION_FORWARD; } else { tDirection = DIRECTION_BACKWARD; - aRequestedSpeed = -aRequestedSpeed; + aRequestedSpeedPWM = -aRequestedSpeedPWM; } - setSpeedCompensated(aRequestedSpeed, tDirection); + setSpeedPWMCompensated(aRequestedSpeedPWM, tDirection); } /* @@ -280,20 +320,19 @@ void PWMDcMotor::setSpeedCompensated(int aRequestedSpeed) { */ void PWMDcMotor::stop(uint8_t aStopMode) { - CurrentSpeed = 0; // The only statement which sets CurrentSpeed to 0 - SpeedOrMotorModeHasChanged = true; - MotorMovesFixedDistance = false; -#ifdef SUPPORT_RAMP_UP + CurrentSpeedPWM = 0; // The only statement which sets CurrentSpeedPWM to 0 + MotorPWMHasChanged = true; + CheckDistanceInUpdateMotor = false; MotorRampState = MOTOR_STATE_STOPPED; -#endif + #ifndef USE_ENCODER_MOTOR_CONTROL - MotorMovesFixedDistance = false; + CheckDistanceInUpdateMotor = false; #endif #ifdef USE_ADAFRUIT_MOTOR_SHIELD # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD - I2CSetPWM(PWMPin, 0, 0); + PCA9685SetPWM(PWMPin, 0, 0); # else - Adafruit_MotorShield_DcMotor->setSpeed(0); + Adafruit_MotorShield_DcMotor->setSpeedPWM(0); # endif #else analogWrite(PWMPin, 0); @@ -301,267 +340,374 @@ void PWMDcMotor::stop(uint8_t aStopMode) { if (aStopMode == STOP_MODE_KEEP) { aStopMode = DefaultStopMode; } - setMotorDriverMode(CheckStopMODE(aStopMode)); + setMotorDriverMode(ForceStopMODE(aStopMode)); +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Stop motor StopMode=")); + Serial.println(ForceStopMODE(aStopMode)); +#endif } /* * @param aStopMode used for speed == 0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE */ void PWMDcMotor::setStopMode(uint8_t aStopMode) { - DefaultStopMode = CheckStopMODE(aStopMode); - MotorValuesHaveChanged = true; + DefaultStopMode = ForceStopMODE(aStopMode); + MotorControlValuesHaveChanged = true; } /****************************************************************************************** * Distance functions *****************************************************************************************/ /* - * StartSpeed (at which car starts to move) for 8 volt is appr. 35 to 40, for 4.3 volt (USB supply) is appr. 90 to 100 - * setDefaultsForFixedDistanceDriving() is called at init by readMotorValuesFromEeprom() if MotorValuesEepromStorageNumber == 0 + * setDefaultsForFixedDistanceDriving() is called at init */ void PWMDcMotor::setDefaultsForFixedDistanceDriving() { - StartSpeed = DEFAULT_START_SPEED; - DriveSpeed = DEFAULT_DRIVE_SPEED; - SpeedCompensation = 0; + StartSpeedPWM = DEFAULT_START_SPEED_PWM; + DriveSpeedPWM = DEFAULT_DRIVE_SPEED_PWM; + SpeedPWMCompensation = 0; #ifndef USE_ENCODER_MOTOR_CONTROL - DistanceToTimeFactor = DEFAULT_DISTANCE_TO_TIME_FACTOR; + MillisPerMillimeter = DEFAULT_MILLIS_PER_MILLIMETER; #endif - MotorValuesHaveChanged = true; + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, uint8_t aSpeedCompensation) { - StartSpeed = aStartSpeed; - DriveSpeed = aDriveSpeed; - SpeedCompensation = aSpeedCompensation; - MotorValuesHaveChanged = true; +void PWMDcMotor::setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, uint8_t aSpeedPWMCompensation) { + StartSpeedPWM = aStartSpeedPWM; + DriveSpeedPWM = aDriveSpeedPWM; + SpeedPWMCompensation = aSpeedPWMCompensation; + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::setSpeedCompensation(uint8_t aSpeedCompensation) { - SpeedCompensation = aSpeedCompensation; - MotorValuesHaveChanged = true; +void PWMDcMotor::setSpeedPWMCompensation(uint8_t aSpeedPWMCompensation) { + SpeedPWMCompensation = aSpeedPWMCompensation; + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::setDriveSpeed(uint8_t aDriveSpeed) { - DriveSpeed = aDriveSpeed; - MotorValuesHaveChanged = true; +void PWMDcMotor::setStartSpeedPWM(uint8_t aStartSpeedPWM) { + StartSpeedPWM = aStartSpeedPWM; + MotorControlValuesHaveChanged = true; } -#ifdef SUPPORT_RAMP_UP -void PWMDcMotor::startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection) { - checkAndHandleDirectionChange(aRequestedDirection); - if (MotorRampState == MOTOR_STATE_STOPPED) { - MotorRampState = MOTOR_STATE_START; +void PWMDcMotor::setDriveSpeedPWM(uint8_t aDriveSpeedPWM) { + DriveSpeedPWM = aDriveSpeedPWM; + MotorControlValuesHaveChanged = true; +} + +void PWMDcMotor::startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection) { +#ifdef DEBUG + Serial.print(PWMPin); + Serial.print(F(" Ramp up to ")); + Serial.print(aRequestedSpeedPWM); + Serial.print(F(" Dir=")); + Serial.print(aRequestedDirection); + Serial.print(F(" CurrentSpeedPWM=")); + Serial.print(CurrentSpeedPWM); + Serial.print(F(" MotorMode=")); + Serial.print(CurrentDirectionOrBrakeMode); + Serial.println(); +#endif - // avoid underflow - uint8_t tCurrentDriveSpeed; - if (aRequestedSpeed > SpeedCompensation) { - tCurrentDriveSpeed = aRequestedSpeed - SpeedCompensation; + if (CurrentSpeedPWM == 0) { // equivalent to MotorRampState == MOTOR_STATE_STOPPED + checkAndHandleDirectionChange(aRequestedDirection); + MotorRampState = MOTOR_STATE_START; + /* + * Set target speed for ramp up + */ + uint8_t tRequestedDriveSpeedPWM; + if (aRequestedSpeedPWM > SpeedPWMCompensation) { + tRequestedDriveSpeedPWM = aRequestedSpeedPWM - SpeedPWMCompensation; } else { - tCurrentDriveSpeed = 0; + tRequestedDriveSpeedPWM = 0; } - CurrentDriveSpeed = tCurrentDriveSpeed; - } else if (MotorRampState == MOTOR_STATE_DRIVE_SPEED) { - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); + RequestedDriveSpeedPWM = tRequestedDriveSpeedPWM; + RampSpeedPWMDelta = RAMP_UP_VALUE_DELTA; // 20V / s + } else if (MotorRampState == MOTOR_STATE_DRIVE) { + // motor is running, -> just change speed + setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); } + // else ramp is in mode MOTOR_STATE_RAMP_UP -> do nothing, let the ramp go on +# ifdef DEBUG + Serial.print(F("MotorRampState=")); + Serial.print(MotorRampState); + Serial.println(); +# endif } void PWMDcMotor::startRampUp(uint8_t aRequestedDirection) { - startRampUp(DriveSpeed, aRequestedDirection); + startRampUp(DriveSpeedPWM, aRequestedDirection); +} + +void PWMDcMotor::startRampDown() { + MotorRampState = MOTOR_STATE_RAMP_DOWN; +// set only the variables for later evaluation in updateMotor() below + if (CurrentSpeedPWM > (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA)) { + CurrentSpeedPWM -= (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA); // RAMP_DOWN_VALUE_DELTA is immediately subtracted below + } else { + CurrentSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; + } } -#endif #if !defined(USE_ENCODER_MOTOR_CONTROL) +/* + * @return true if not stopped (motor expects another update) + */ +bool PWMDcMotor::updateMotor() { + unsigned long tMillis = millis(); + uint8_t tNewSpeedPWM = CurrentSpeedPWM; + + if (MotorRampState == MOTOR_STATE_START) { + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; + /* + * Start motor + */ + if (RequestedDriveSpeedPWM > RAMP_VALUE_UP_OFFSET_SPEED_PWM) { + // start with ramp to avoid spinning wheels + tNewSpeedPWM = RAMP_VALUE_UP_OFFSET_SPEED_PWM; // start immediately with speed offset (3 volt) + // --> RAMP_UP + MotorRampState = MOTOR_STATE_RAMP_UP; + } else { + // Motor ramp not required, go direct to drive speed. + tNewSpeedPWM = RequestedDriveSpeedPWM; + // --> DRIVE + MotorRampState = MOTOR_STATE_DRIVE; + } + } else if (MotorRampState == MOTOR_STATE_RAMP_UP) { + /* + * Increase motor speed by RAMP_UP_VALUE_DELTA every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + */ + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis += RAMP_INTERVAL_MILLIS; + tNewSpeedPWM = tNewSpeedPWM + RampSpeedPWMDelta; + /* + * Transition criteria is: RequestedDriveSpeedPWM reached. + * Then check immediately for timeout + */ + // Clip value and check for 8 bit overflow + if (tNewSpeedPWM >= RequestedDriveSpeedPWM || tNewSpeedPWM <= RampSpeedPWMDelta) { + tNewSpeedPWM = RequestedDriveSpeedPWM; + // --> DRIVE + MotorRampState = MOTOR_STATE_DRIVE; + } + } + } + + if (MotorRampState == MOTOR_STATE_DRIVE) { + /* + * Time based distance. Ramp down is included in time formula. + */ + if (CheckDistanceInUpdateMotor && millis() >= computedMillisOfMotorStopForDistance) { + + if (tNewSpeedPWM > (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA)) { + tNewSpeedPWM -= (RAMP_VALUE_DOWN_OFFSET_SPEED_PWM - RAMP_DOWN_VALUE_DELTA); // RAMP_DOWN_VALUE_DELTA is immediately subtracted below + // --> RAMP_DOWN + MotorRampState = MOTOR_STATE_RAMP_DOWN; + } else { + // --> STOPPED + tNewSpeedPWM = 0; + } + } + } + + if (MotorRampState == MOTOR_STATE_RAMP_DOWN) { + if (tMillis >= NextRampChangeMillis) { + NextRampChangeMillis = tMillis + RAMP_INTERVAL_MILLIS; + /* + * Decrease motor speed RAMP_UP_UPDATE_INTERVAL_STEPS times every RAMP_UP_UPDATE_INTERVAL_MILLIS milliseconds + */ + if (tNewSpeedPWM > (RAMP_DOWN_VALUE_DELTA + RAMP_VALUE_DOWN_MIN_SPEED_PWM)) { + tNewSpeedPWM -= RAMP_DOWN_VALUE_DELTA; + } else { + tNewSpeedPWM = RAMP_VALUE_DOWN_MIN_SPEED_PWM; + } + } + } +// End of motor state machine + +#ifdef TRACE + Serial.print(F("St=")); + Serial.println(MotorRampState); +#endif + if (tNewSpeedPWM != CurrentSpeedPWM) { +#ifdef TRACE + Serial.print(F("Ns=")); + Serial.println(tNewSpeedPWM); +#endif + PWMDcMotor::setSpeedPWM(tNewSpeedPWM, CurrentDirectionOrBrakeMode); // sets MOTOR_STATE_STOPPED if speed is 0 + } + + /* + * Check if target milliseconds are reached + */ + if (CurrentSpeedPWM > 0) { + if (CheckDistanceInUpdateMotor && millis() > computedMillisOfMotorStopForDistance) { + stop(DefaultStopMode); // resets CheckDistanceInUpdateMotor and sets MOTOR_STATE_STOPPED; + return false; // need no more calls to update() + } + return true; // still running + } + return false; // current speed == 0 +} + /* * Required for non encoder motors to estimate duration for a fixed distance */ -void PWMDcMotor::setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor) { - DistanceToTimeFactor = aDistanceToTimeFactor; +void PWMDcMotor::setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond) { + MillisPerMillimeter = 1000 / aMillimeterPerSecond; } /* - * @param aRequestedDistanceCount distance in 5mm resolution (to be compatible with 20 slot encoder discs and 20 cm wheel circumference) * If motor is already running just update speed and new time */ -void PWMDcMotor::startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { -// if (aRequestedDistanceCount > DEFAULT_COUNTS_PER_FULL_ROTATION * 10) { -// PanicWithLed(400, 22); -// } - if (aRequestedDistanceCount == 0) { +void PWMDcMotor::startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + if (aRequestedDistanceMillimeter == 0) { + stop(DefaultStopMode); // In case motor was running return; } - if (CurrentSpeed == 0) { -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; - CurrentDriveSpeed = aRequestedSpeed; - setMotorDriverMode(aRequestedDirection); // this in turn sets CurrentDirectionOrBrakeMode -#else - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); -#endif + if (CurrentSpeedPWM == 0) { + startRampUp(aRequestedSpeedPWM, aRequestedDirection); + /* - * Estimate duration for given distance - * use 32 bit intermediate to avoid overflow (this also saves around 50 bytes of program memory by using slower functions instead of faster inline code) + * Estimate duration for given distance# + * MillisPerMillimeter is defined for DEFAULT_DRIVE_SPEED_PWM is */ - computedMillisOfMotorStopForDistance = millis() + DEFAULT_MOTOR_START_UP_TIME_MILLIS - + (10 * (((uint32_t) aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); + if (aRequestedSpeedPWM == DEFAULT_DRIVE_SPEED_PWM) { + computedMillisOfMotorStopForDistance = millis() + DEFAULT_MOTOR_START_UP_TIME_MILLIS + + (((uint16_t) aRequestedDistanceMillimeter * MillisPerMillimeter)); + } else { + computedMillisOfMotorStopForDistance = millis() + DEFAULT_MOTOR_START_UP_TIME_MILLIS + + (((uint32_t) aRequestedDistanceMillimeter * MillisPerMillimeter * DriveSpeedPWM) / DEFAULT_DRIVE_SPEED_PWM); + } + } else { -#ifdef SUPPORT_RAMP_UP - MotorRampState = MOTOR_STATE_START; - CurrentDriveSpeed = aRequestedSpeed; -#endif - setSpeedCompensated(aRequestedSpeed, aRequestedDirection); + MotorRampState = MOTOR_STATE_DRIVE; + setSpeedPWMCompensated(aRequestedSpeedPWM, aRequestedDirection); /* * Estimate duration for given distance * use 32 bit intermediate to avoid overflow (this also saves around 50 bytes of program memory by using slower functions instead of faster inline code) */ computedMillisOfMotorStopForDistance = millis() - + (10 * (((uint32_t) aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); + + (((uint32_t) aRequestedDistanceMillimeter * MillisPerMillimeter * DriveSpeedPWM) / DEFAULT_DRIVE_SPEED_PWM); } - MotorMovesFixedDistance = true; + CheckDistanceInUpdateMotor = true; } -void PWMDcMotor::startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +void PWMDcMotor::startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } /* * Signed DistanceCount */ -void PWMDcMotor::startGoDistanceCount(int aRequestedDistanceCount) { - if (aRequestedDistanceCount < 0) { - aRequestedDistanceCount = -aRequestedDistanceCount; - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, DIRECTION_BACKWARD); +void PWMDcMotor::startGoDistanceMillimeter(int aRequestedDistanceMillimeter) { + if (aRequestedDistanceMillimeter < 0) { + aRequestedDistanceMillimeter = -aRequestedDistanceMillimeter; + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_BACKWARD); } else { - startGoDistanceCount(DriveSpeed, aRequestedDistanceCount, DIRECTION_FORWARD); + startGoDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, DIRECTION_FORWARD); } } /* * Not used by CarControl */ -void PWMDcMotor::goDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - goDistanceCount(DriveSpeed, aRequestedDistanceCount, aRequestedDirection); +void PWMDcMotor::goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection) { + goDistanceMillimeter(DriveSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); } -void PWMDcMotor::goDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection) { - startGoDistanceCount(aRequestedSpeed, aRequestedDistanceCount, aRequestedDirection); +void PWMDcMotor::goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, + uint8_t aRequestedDirection) { + startGoDistanceMillimeter(aRequestedSpeedPWM, aRequestedDistanceMillimeter, aRequestedDirection); while (millis() <= computedMillisOfMotorStopForDistance) { -#ifdef SUPPORT_RAMP_UP updateMotor(); -#endif } stop(DefaultStopMode); } -/* - * @return true if not stopped (motor expects another update) - */ -bool PWMDcMotor::updateMotor() { -#ifdef SUPPORT_RAMP_UP - unsigned long tMillis = millis(); - uint8_t tNewSpeed = CurrentSpeed; - - if (MotorRampState == MOTOR_STATE_START) { - // --> RAMP_UP - MotorRampState = MOTOR_STATE_RAMP_UP; - /* - * Set ramp values, 16 steps a 16 millis for ramp up => 256 milliseconds - */ - NextRampChangeMillis = tMillis + RAMP_UP_UPDATE_INTERVAL_MILLIS; - RampDelta = RAMP_UP_VALUE_DELTA; // ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS) - if (RampDelta < 2) { - RampDelta = 2; - } - /* - * Start motor - */ - tNewSpeed = StartSpeed; - } - - // do not use else if since state can be changed in code before - if (MotorRampState == MOTOR_STATE_RAMP_UP) { - /* - * Increase motor speed RAMP_UP_UPDATE_INTERVAL_STEPS (16) times every RAMP_UP_UPDATE_INTERVAL_MILLIS (16) milliseconds - * or until more than half of distance is done - * Distance required for ramp is 0 to 10 or more, increasing with increasing CurrentDriveSpeed - */ - if (tMillis >= NextRampChangeMillis) { - NextRampChangeMillis += RAMP_UP_UPDATE_INTERVAL_MILLIS; - tNewSpeed = tNewSpeed + RampDelta; - // Clip value and check for 8 bit overflow - if (tNewSpeed > CurrentDriveSpeed || tNewSpeed <= RampDelta) { - tNewSpeed = CurrentDriveSpeed; - } - - /* - * Transition criteria is: - * Max Speed reached or more than half of distance is done - */ - if (tNewSpeed == CurrentDriveSpeed) { - // --> DRIVE_SPEED - MotorRampState = MOTOR_STATE_DRIVE_SPEED; - } - } - } - // End of motor state machine - - if (tNewSpeed != CurrentSpeed) { - PWMDcMotor::setSpeed(tNewSpeed, CurrentDirectionOrBrakeMode); - } -#endif - - /* - * Check if target milliseconds are reached - */ - if (CurrentSpeed > 0) { - if (MotorMovesFixedDistance && millis() > computedMillisOfMotorStopForDistance) { - stop(DefaultStopMode); // resets MotorMovesFixedDistance - return false; - } - } - return true; -} #endif // !defined(USE_ENCODER_MOTOR_CONTROL) /******************************************************************************************** * EEPROM functions * Uses the start of EEPROM for storage of EepromMotorInfoStruct's for motor number 1 to n - * setDefaultsForFixedDistanceDriving() is called by readMotorValuesFromEeprom() if MotorValuesEepromStorageNumber == 0 ********************************************************************************************/ -void PWMDcMotor::readMotorValuesFromEeprom() { - if (MotorValuesEepromStorageNumber != 0) { - EepromMotorInfoStruct tEepromMotorInfo; - eeprom_read_block((void*) &tEepromMotorInfo, (void*) ((MotorValuesEepromStorageNumber - 1) * sizeof(EepromMotorInfoStruct)), - sizeof(EepromMotorInfoStruct)); +void PWMDcMotor::readMotorValuesFromEeprom(uint8_t aMotorValuesEepromStorageNumber) { + EepromMotorInfoStruct tEepromMotorInfo; + eeprom_read_block((void*) &tEepromMotorInfo, (void*) ((aMotorValuesEepromStorageNumber) * sizeof(EepromMotorInfoStruct)), + sizeof(EepromMotorInfoStruct)); - /* - * Overwrite with values if valid - */ - if (tEepromMotorInfo.StartSpeed < 150 && tEepromMotorInfo.StartSpeed > 20) { - StartSpeed = tEepromMotorInfo.StartSpeed; - if (tEepromMotorInfo.DriveSpeed > 40) { - DriveSpeed = tEepromMotorInfo.DriveSpeed; - } - if (tEepromMotorInfo.SpeedCompensation < 24) { - SpeedCompensation = tEepromMotorInfo.SpeedCompensation; - } + /* + * Overwrite with values if valid + */ + if (tEepromMotorInfo.StartSpeedPWM < 150 && tEepromMotorInfo.StartSpeedPWM > 20) { + StartSpeedPWM = tEepromMotorInfo.StartSpeedPWM; + if (tEepromMotorInfo.DriveSpeedPWM > 40) { + DriveSpeedPWM = tEepromMotorInfo.DriveSpeedPWM; + } + if (tEepromMotorInfo.SpeedPWMCompensation < 24) { + SpeedPWMCompensation = tEepromMotorInfo.SpeedPWMCompensation; } - MotorValuesHaveChanged = true; - } else { - setDefaultsForFixedDistanceDriving(); } + MotorControlValuesHaveChanged = true; } -void PWMDcMotor::writeMotorvaluesToEeprom() { - if (MotorValuesEepromStorageNumber != 0) { - EepromMotorInfoStruct tEepromMotorInfo; - tEepromMotorInfo.StartSpeed = StartSpeed; - tEepromMotorInfo.DriveSpeed = DriveSpeed; - tEepromMotorInfo.SpeedCompensation = SpeedCompensation; +void PWMDcMotor::writeMotorValuesToEeprom(uint8_t aMotorValuesEepromStorageNumber) { + EepromMotorInfoStruct tEepromMotorInfo; + tEepromMotorInfo.StartSpeedPWM = StartSpeedPWM; + tEepromMotorInfo.DriveSpeedPWM = DriveSpeedPWM; + tEepromMotorInfo.SpeedPWMCompensation = SpeedPWMCompensation; - eeprom_write_block((void*) &tEepromMotorInfo, - (void*) ((MotorValuesEepromStorageNumber - 1) * sizeof(EepromMotorInfoStruct)), sizeof(EepromMotorInfoStruct)); - } + eeprom_write_block((void*) &tEepromMotorInfo, (void*) ((aMotorValuesEepromStorageNumber) * sizeof(EepromMotorInfoStruct)), + sizeof(EepromMotorInfoStruct)); +} + +const char StringNot[] PROGMEM = { " not" }; +const char StringDefined[] PROGMEM = { " defined" }; + +void PWMDcMotor::printSettings(Print *aSerial) { + aSerial->println(); + aSerial->println(F("Settings from PWMDcMotor.h:")); + + aSerial->print(F("USE_ENCODER_MOTOR_CONTROL:")); +#ifndef USE_ENCODER_MOTOR_CONTROL + aSerial->print(reinterpret_cast(StringNot)); +#endif + aSerial->println(reinterpret_cast(StringDefined)); + + aSerial->print(F("USE_ADAFRUIT_MOTOR_SHIELD:")); +#ifndef USE_ADAFRUIT_MOTOR_SHIELD + aSerial->print(reinterpret_cast(StringNot)); +#endif + aSerial->println(reinterpret_cast(StringDefined)); + +#ifdef USE_ADAFRUIT_MOTOR_SHIELD + aSerial->print(F("USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD:")); +#ifndef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD + aSerial->print(reinterpret_cast(StringNot)); +#endif + aSerial->println(reinterpret_cast(StringDefined)); +#endif + + aSerial->print(F("DEFAULT_MOTOR_START_UP_TIME_MILLIS=")); + aSerial->println(DEFAULT_MOTOR_START_UP_TIME_MILLIS); + + aSerial->print(F("FULL_BRIDGE_OUTPUT_MILLIVOLT=")); + aSerial->print(FULL_BRIDGE_OUTPUT_MILLIVOLT); + aSerial->print(F("mV (= FULL_BRIDGE_INPUT_MILLIVOLT|")); + aSerial->print(FULL_BRIDGE_INPUT_MILLIVOLT); + aSerial->print(F("mV - FULL_BRIDGE_LOSS_MILLIVOLT|")); + aSerial->print(FULL_BRIDGE_LOSS_MILLIVOLT); + aSerial->println(F("mV)")); + + aSerial->print(F("DEFAULT_START_SPEED_PWM=")); + aSerial->print(DEFAULT_START_SPEED_PWM); + aSerial->print(F(", DEFAULT_DRIVE_SPEED_PWM=")); + aSerial->println(DEFAULT_DRIVE_SPEED_PWM); + + aSerial->print(F("DEFAULT_MILLIS_PER_MILLIMETER=")); + aSerial->println(DEFAULT_MILLIS_PER_MILLIMETER); + aSerial->println(); } //void PanicWithLed(unsigned int aDelay, uint8_t aCount) { diff --git a/src/PWMDcMotor.h b/src/PWMDcMotor.h index 8c006d8..32b2e07 100644 --- a/src/PWMDcMotor.h +++ b/src/PWMDcMotor.h @@ -1,10 +1,18 @@ /* * PWMDCMotor.h * - * Motor control has 2 technical dimensions - * 1. Motor driver control / direction. Can be FORWARD, BACKWARD, BRAKE (motor connections are shortened) - * or RELEASE (motor connections are high impedance). - * 2. Speed / PWM which is ignored for BRAKE or RELEASE. + * Motor control has 2 parameters: + * 1. SpeedPWM / PWM which is ignored for BRAKE or RELEASE. This library also accepts signed speed (including the direction as sign). + * 2. Direction / MotorDriverMode. Can be FORWARD, BACKWARD (BRAKE motor connection are shortened) or RELEASE ( motor connections are high impedance) + * + * PWM period is 600 us for Adafruit Motor Shield V2 using PCA9685. + * PWM period is 1030 us for using AnalogWrite on pin 5 + 6. + * + * Distance is computed in 3 different ways. + * Without IMU or Encoder: - distance is converted to a time for riding. + * With IMU: - distance is measured by IMU. + * With encoder: - distance is measured by Encoder. + * * * Created on: 12.05.2019 * Copyright (C) 2019-2020 Armin Joachimsmeyer @@ -12,6 +20,11 @@ * * This file is part of PWMMotorControl https://github.com/ArminJo/PWMMotorControl. * + * PWMMotorControl is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the @@ -26,24 +39,33 @@ #include -#define VERSION_PWMMOTORCONTROL "1.1.1" -#define VERSION_PWMMOTORCONTROL_MAJOR 1 -#define VERSION_PWMMOTORCONTROL_MINOR 1 +#define VERSION_PWMMOTORCONTROL "2.0.0" +#define VERSION_PWMMOTORCONTROL_MAJOR 2 +#define VERSION_PWMMOTORCONTROL_MINOR 0 +// The change log is at the bottom of the file /* - * Comment this out, if you have encoder interrupts attached at pin 2 and 3 + * Activate this, if you have encoder interrupts attached at pin 2 and 3 * and want to use the methods of the EncoderMotor class for fixed distance / closed loop driving. * Enabling it will disable no longer required PWMDCMotor class variables and functions - * and force the usage of the EncoderMotor class in CarMotorControl. + * and force the usage of the EncoderMotor class in CarPWMMotorControl. */ //#define USE_ENCODER_MOTOR_CONTROL // /* - * Use Adafruit Motor Shield v2 connected by I2C instead of simple TB6612 or L298 breakout board. - * This disables tone output by using motor as loudspeaker, but requires only 2 I2C/TWI pins in contrast to the 6 pins used for the full bridge. + * Use Adafruit Motor Shield v2 connected by I2C instead of TB6612 or L298 breakout board. + * This disables using motor as buzzer, but requires only 2 I2C/TWI pins in contrast to the 6 pins used for the full bridge. * For full bridge, analogWrite the millis() timer0 is used since we use pin 5 & 6. */ //#define USE_ADAFRUIT_MOTOR_SHIELD +#if defined(USE_ADAFRUIT_MOTOR_SHIELD) +# if !defined(MOSFET_BRIDGE_USED) +#define MOSFET_BRIDGE_USED +# endif +# if !defined(FULL_BRIDGE_LOSS_MILLIVOLT) +#define FULL_BRIDGE_LOSS_MILLIVOLT 0 +# endif +#endif // /* * Own library saves me 694 bytes program memory @@ -53,65 +75,88 @@ #endif /* - * Disabling SUPPORT_RAMP_UP saves 7 bytes RAM per motor and around 300 bytes program memory + * Activate this, if you use default settings and 2 LiPo Cells (around 7.4 volt) as Motor supply. */ -#if ! DO_NOT_SUPPORT_RAMP_UP // if defined (externally), disables ramp up support -#define SUPPORT_RAMP_UP // 256 milliseconds for ramp, see below -#endif - +//#define VIN_2_LIPO /* - * Comment this out, if you use default settings and 2 LiPo Cells (around 7.4 volt) as Motor supply. + * Helper macro for getting a macro definition as string */ -//#define VIN_2_LIPO -#define MAX_SPEED 255 +#define STR_HELPER(x) #x +#define STR(x) STR_HELPER(x) + +#define MAX_SPEED_PWM 255 /* - * This values are chosen to be compatible with 20 slot encoder discs, giving 20 on and 20 off counts per full rotation. - * At a circumference of around 20 cm (21.5 cm actual) this gives 5 mm per count. + * Circumference of my smart car wheel */ -#define DEFAULT_COUNTS_PER_FULL_ROTATION 40 -#define DEFAULT_MILLIMETER_PER_COUNT 5 +#define DEFAULT_CIRCUMFERENCE_MILLIMETER 220 -#define DEFAULT_MOTOR_START_UP_TIME_MILLIS 150 // constant value for the for the formula below /* - * DEFAULT_DISTANCE_TO_TIME_FACTOR is the factor used to convert distance in 5mm steps to motor on time in milliseconds. I depends on motor supply voltage. - * Currently formula is: - * computedMillisOfMotorStopForDistance = DEFAULT_MOTOR_START_UP_TIME_MILLIS + (10 * ((aRequestedDistanceCount * DistanceToTimeFactor) / DriveSpeed)); - * - * DEFAULT_START_SPEED is the speed PWM value at which car starts to move. For 8 volt is appr. 35 to 40, for 3,6 volt (USB supply) is appr. 70 to 100 + * I measured maximum positive acceleration with spinning wheels as 250 cm/s^2 on varnished wood. + * I measured maximum negative acceleration with blocking wheels as 350 cm/s^2 on varnished wood. + * This corresponds to 5 cm/s every 20 ms */ -#define DEFAULT_START_SPEED_7_4_VOLT 45 -#define DEFAULT_DRIVE_SPEED_7_4_VOLT 80 -#define DEFAULT_DISTANCE_TO_TIME_FACTOR_7_4_VOLT 135 // for 2 x LIPO batteries (7.4 volt). - -#define DEFAULT_START_SPEED_6_VOLT 140 -#define DEFAULT_DRIVE_SPEED_6_VOLT 220 -#define DEFAULT_DISTANCE_TO_TIME_FACTOR_6_VOLT 300 // for 4 x AA batteries (6 volt). - -// Default values - used if EEPROM values are invalid -#if defined(VIN_2_LIPO) -# if !defined(DEFAULT_START_SPEED) -#define DEFAULT_START_SPEED DEFAULT_START_SPEED_7_4_VOLT -# endif -# if !defined(DEFAULT_DRIVE_SPEED) -#define DEFAULT_DRIVE_SPEED DEFAULT_DRIVE_SPEED_7_4_VOLT -# endif -# if !defined(DEFAULT_DISTANCE_TO_TIME_FACTOR) -#define DEFAULT_DISTANCE_TO_TIME_FACTOR DEFAULT_DISTANCE_TO_TIME_FACTOR_7_4_VOLT +#define RAMP_INTERVAL_MILLIS 20 // The smaller the value the steeper the ramp +#define RAMP_VALUE_UP_OFFSET_MILLIVOLT 2300 // Start positive or negative acceleration with this voltage offset in order to get a reasonable acceleration for ramps +#define RAMP_VALUE_DOWN_OFFSET_MILLIVOLT 2500 // Start positive or negative acceleration with this voltage offset in order to get a reasonable acceleration for ramps +#define RAMP_VALUE_UP_OFFSET_SPEED_PWM ((RAMP_VALUE_UP_OFFSET_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define RAMP_VALUE_DOWN_OFFSET_SPEED_PWM ((RAMP_VALUE_DOWN_OFFSET_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define RAMP_VALUE_DOWN_MIN_SPEED_PWM (((DEFAULT_DRIVE_MILLIVOLT / 2) * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define RAMP_UP_VALUE_DELTA (SPEED_FOR_8_VOLT / (1000 / RAMP_INTERVAL_MILLIS)) // Results in a ramp up voltage of 10V/s = 1.0 volt per 100 ms +#define RAMP_DOWN_VALUE_DELTA ((SPEED_FOR_8_VOLT * 2)/ (1000 / RAMP_INTERVAL_MILLIS)) // Results in a ramp up voltage of 20V/s = 1.0 volt per 100 ms +#define RAMP_DECELERATION_TIMES_2 3500 // Take half of the observed maximum. This depends on the type of tires and the mass of the car + +#define DEFAULT_MOTOR_START_UP_TIME_MILLIS 15 // 15 to 20, constant value for the for the formula below + +#if !defined(FULL_BRIDGE_INPUT_MILLIVOLT) +# if defined(VIN_2_LIPO) +#define FULL_BRIDGE_INPUT_MILLIVOLT 7400 // for 2 x LIPO batteries (7.4 volt). +# else +#define FULL_BRIDGE_INPUT_MILLIVOLT 6000 // for 4 x AA batteries (6 volt). # endif +#endif -#else -# if !defined(DEFAULT_START_SPEED) -#define DEFAULT_START_SPEED DEFAULT_START_SPEED_6_VOLT -# endif -# if !defined(DEFAULT_DRIVE_SPEED) -#define DEFAULT_DRIVE_SPEED DEFAULT_DRIVE_SPEED_6_VOLT -# endif -# if !defined(DEFAULT_DISTANCE_TO_TIME_FACTOR) -#define DEFAULT_DISTANCE_TO_TIME_FACTOR DEFAULT_DISTANCE_TO_TIME_FACTOR_6_VOLT +#if !defined(FULL_BRIDGE_LOSS_MILLIVOLT) +# if defined(MOSFET_BRIDGE_USED) +// Speed is almost linear to 1/2 PWM in cm/s without any offset, only with dead band +#define FULL_BRIDGE_LOSS_MILLIVOLT 0 +# else +// Speed is not linear to PWM and has an offset +// Effective voltage loss includes loss by switching to high impedance at inactive for bipolar full bridges like L298 +#define FULL_BRIDGE_LOSS_MILLIVOLT 2200 // Effective voltage loss # endif #endif +#if !defined(FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define FULL_BRIDGE_OUTPUT_MILLIVOLT (FULL_BRIDGE_INPUT_MILLIVOLT - FULL_BRIDGE_LOSS_MILLIVOLT) +#endif + +#define DEFAULT_START_MILLIVOLT 1100 // Start voltage -motors start to turn- is 1.1 volt +#define DEFAULT_DRIVE_MILLIVOLT 2000 // Drive voltage is 2.0 volt +#define SPEED_FOR_1_VOLT ((1000L * MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define SPEED_FOR_8_VOLT ((8000L * MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#define SPEED_FOR_10_VOLT ((10000L * MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) + +// Default values - used if EEPROM values are invalid or nor available +#if !defined(DEFAULT_START_SPEED_PWM) +// DEFAULT_START_SPEED_PWM is the speed PWM value at which motor starts to move. 70|127 for 4 volt 37|68 for 7.4 volt +#define DEFAULT_START_SPEED_PWM ((DEFAULT_START_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#endif +#if !defined(DEFAULT_DRIVE_SPEED_PWM) +// At 2 volt I measured around 32 cm/s. 68 for 7.4 volt +#define DEFAULT_DRIVE_SPEED_PWM ((DEFAULT_DRIVE_MILLIVOLT * (long)MAX_SPEED_PWM) / FULL_BRIDGE_OUTPUT_MILLIVOLT) +#endif + +#if !defined(DEFAULT_MILLIMETER_PER_SECOND) +// At 2 volt (DEFAULT_DRIVE_MILLIVOLT) we have around 1.25 rotation per second -> 25 distance/encoder counts per second -> 27 cm / second +#define DEFAULT_MILLIMETER_PER_SECOND 320 // at DEFAULT_DRIVE_MILLIVOLT motor supply +#define DEFAULT_MILLIS_PER_MILLIMETER (1000 / DEFAULT_MILLIMETER_PER_SECOND) +#endif +/* + * Currently formula used to convert distance in 11 mm steps to motor on time in milliseconds is: + * computedMillisOfMotorStopForDistance = DEFAULT_MOTOR_START_UP_TIME_MILLIS + (((aRequestedDistanceCount * MillisPerMillimeter) / DriveSpeedPWM)); + */ + // Motor directions and stop modes. Are used for parameter aMotorDriverMode and sequence is determined by the Adafruit library API. #define DIRECTION_FORWARD 0 #define DIRECTION_BACKWARD 1 @@ -124,22 +169,22 @@ #define STOP_MODE_AND_MASK 0x03 #define STOP_MODE_OR_MASK 0x02 #define DEFAULT_STOP_MODE MOTOR_RELEASE -#define CheckStopMODE(aStopMode) ((aStopMode & STOP_MODE_AND_MASK) | STOP_MODE_OR_MASK) +#define ForceStopMODE(aStopMode) ((aStopMode & STOP_MODE_AND_MASK) | STOP_MODE_OR_MASK) #ifdef USE_ADAFRUIT_MOTOR_SHIELD #include # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD // some PCA9685 specific constants +#define PCA9685_DEFAULT_ADDRESS 0x60 #define PCA9685_GENERAL_CALL_ADDRESS 0x00 -#define PCA9685_SOFTWARE_RESET 6 -#define PCA9685_DEFAULT_ADDRESS 0x40 -#define PCA9685_MAX_CHANNELS 16 // 16 PWM channels on each PCA9685 expansion module -#define PCA9685_MODE1_REGISTER 0x0 -#define PCA9685_MODE_1_RESTART 7 -#define PCA9685_MODE_1_AUTOINCREMENT 5 -#define PCA9685_MODE_1_SLEEP 4 -#define PCA9685_FIRST_PWM_REGISTER 0x06 -#define PCA9685_PRESCALE_REGISTER 0xFE +#define PCA9685_SOFTWARE_RESET 6 +#define PCA9685_MAX_CHANNELS 16 // 16 PWM channels on each PCA9685 expansion module +#define PCA9685_MODE1_REGISTER 0x00 +#define PCA9685_MODE_1_RESTART 7 +#define PCA9685_MODE_1_AUTOINCREMENT 5 +#define PCA9685_MODE_1_SLEEP 4 +#define PCA9685_FIRST_PWM_REGISTER 0x06 +#define PCA9685_PRESCALE_REGISTER 0xFE #define PCA9685_PRESCALER_FOR_1600_HZ ((25000000L /(4096L * 1600))-1) // = 3 at 1600 Hz @@ -150,93 +195,92 @@ #endif // USE_ADAFRUIT_MOTOR_SHIELD struct EepromMotorInfoStruct { - uint8_t StartSpeed; - uint8_t StopSpeed; - uint8_t DriveSpeed; - uint8_t SpeedCompensation; + uint8_t StartSpeedPWM; + uint8_t StopSpeedPWM; + uint8_t DriveSpeedPWM; + uint8_t SpeedPWMCompensation; }; /* - * For ramp control + * Ramp control + * Ramp up speed in 16 steps every 16 millis from from StartSpeedPWM to RequestedDriveSpeedPWM. */ #define MOTOR_STATE_STOPPED 0 #define MOTOR_STATE_START 1 #define MOTOR_STATE_RAMP_UP 2 -#define MOTOR_STATE_DRIVE_SPEED 3 +#define MOTOR_STATE_DRIVE 3 #define MOTOR_STATE_RAMP_DOWN 4 -#ifdef SUPPORT_RAMP_UP -#define RAMP_UP_UPDATE_INTERVAL_MILLIS 16 // The smaller the value the steeper the ramp -#define RAMP_UP_UPDATE_INTERVAL_STEPS 16 // Results in a ramp up time of 16 steps * 16 millis = 256 milliseconds -#define RAMP_UP_VALUE_DELTA ((CurrentDriveSpeed - StartSpeed) / RAMP_UP_UPDATE_INTERVAL_STEPS) -#endif - class PWMDcMotor { public: PWMDcMotor(); #ifdef USE_ADAFRUIT_MOTOR_SHIELD - void init(uint8_t aMotorNumber, bool aReadFromEeprom = false); + void init(uint8_t aMotorNumber); # ifdef USE_OWN_LIBRARY_FOR_ADAFRUIT_MOTOR_SHIELD /* * Own internal functions for communicating with the PCA9685 Expander IC on the Adafruit motor shield */ - void I2CWriteByte(uint8_t aAddress, uint8_t aData); - void I2CSetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff); - void I2CSetPin(uint8_t aPin, bool aSetToOn); + void PCA9685WriteByte(uint8_t aAddress, uint8_t aData); + void PCA9685SetPWM(uint8_t aPin, uint16_t aOn, uint16_t aOff); + void PCA9685SetPin(uint8_t aPin, bool aSetToOn); # else - Adafruit_DCMotor * Adafruit_MotorShield_DcMotor; + Adafruit_DCMotor *Adafruit_MotorShield_DcMotor; # endif #else - void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin, uint8_t aMotorNumber = 0); + void init(uint8_t aForwardPin, uint8_t aBackwardPin, uint8_t aPWMPin); #endif /* * Basic motor commands */ - void setSpeed(int aRequestedSpeed); - void setSpeed(uint8_t aSpeedRequested, uint8_t aRequestedDirection); - void setSpeedCompensated(int aRequestedSpeed); - void setSpeedCompensated(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); + void setSpeedPWM(int aRequestedSpeedPWM); + void changeSpeedPWM(uint8_t aRequestedSpeedPWM); // Keeps direction + void setSpeedPWM(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void setSpeedPWMCompensated(int aRequestedSpeedPWM); + void changeSpeedPWMCompensated(uint8_t aRequestedSpeedPWM); // Keeps direction + void setSpeedPWMCompensated(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); void stop(uint8_t aStopMode = STOP_MODE_KEEP); // STOP_MODE_KEEP (take previously defined DefaultStopMode) or MOTOR_BRAKE or MOTOR_RELEASE - void setStopMode(uint8_t aStopMode); // mode for Speed==0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE + void setStopMode(uint8_t aStopMode); // mode for SpeedPWM==0 or STOP_MODE_KEEP: MOTOR_BRAKE or MOTOR_RELEASE /* * Fixed distance driving */ - void setValuesForFixedDistanceDriving(uint8_t aStartSpeed, uint8_t aDriveSpeed, uint8_t aSpeedCompensation = 0); + void setValuesForFixedDistanceDriving(uint8_t aStartSpeedPWM, uint8_t aDriveSpeedPWM, uint8_t aSpeedPWMCompensation = 0); void setDefaultsForFixedDistanceDriving(); - void setSpeedCompensation(uint8_t aSpeedCompensation); - void setDriveSpeed(uint8_t aDriveSpeed); + void setSpeedPWMCompensation(uint8_t aSpeedPWMCompensation); + void setStartSpeedPWM(uint8_t aStartSpeedPWM); + void setDriveSpeedPWM(uint8_t aDriveSpeedPWM); -#ifdef SUPPORT_RAMP_UP void startRampUp(uint8_t aRequestedDirection); - void startRampUp(uint8_t aRequestedSpeed, uint8_t aRequestedDirection); -#endif + void startRampUp(uint8_t aRequestedSpeedPWM, uint8_t aRequestedDirection); + void startRampDown(); -#ifndef USE_ENCODER_MOTOR_CONTROL +#ifndef USE_ENCODER_MOTOR_CONTROL // required here, since we cannot access the computedMillisOfMotorStopForDistance and MillisPerMillimeter for the functions below // This function only makes sense for non encoder motors - void setDistanceToTimeFactorForFixedDistanceDriving(unsigned int aDistanceToTimeFactor); + void setMillimeterPerSecondForFixedDistanceDriving(uint16_t aMillimeterPerSecond); // These functions are implemented by encoder motor too - void startGoDistanceCount(int aRequestedDistanceCount); // Signed distance count - void startGoDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); - void startGoDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); + void startGoDistanceMillimeter(int aRequestedDistanceMillimeter); // Signed distance + void startGoDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); + void startGoDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); bool updateMotor(); /* * Implementation for non encoder motors. Not used by CarControl. */ - void goDistanceCount(unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); - void goDistanceCount(uint8_t aRequestedSpeed, unsigned int aRequestedDistanceCount, uint8_t aRequestedDirection); + void goDistanceMillimeter(unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); + void goDistanceMillimeter(uint8_t aRequestedSpeedPWM, unsigned int aRequestedDistanceMillimeter, uint8_t aRequestedDirection); #endif /* - * EEPROM functions to read and store control values (DriveSpeed, SpeedCompensation) + * EEPROM functions to read and store control values (DriveSpeedPWM, SpeedPWMCompensation) */ - void readMotorValuesFromEeprom(); - void writeMotorvaluesToEeprom(); + void readMotorValuesFromEeprom(uint8_t aMotorValuesEepromStorageNumber); + void writeMotorValuesToEeprom(uint8_t aMotorValuesEepromStorageNumber); + + static void printSettings(Print *aSerial); /* * Internal functions @@ -250,56 +294,58 @@ class PWMDcMotor { uint8_t BackwardPin; #endif - int8_t MotorValuesEepromStorageNumber; // number of EEPROM EepromMotorInfoStruct block address for one PWMDcMotor. 0 means no corresponding EEPROM block. Set by init. /********************************** - * Start of EEPROM values + * Start of values for EEPROM *********************************/ /* - * Minimum speed setting at which motor starts moving. Depend on current voltage, load and surface. + * Minimum SpeedPWM setting at which motor starts moving. Depend on current voltage, load and surface. * Is set by calibrate() and then stored (with the other values) in eeprom. */ - uint8_t StartSpeed; // Speed PWM value at which car starts to move. For 8 volt is appr. 35 to 40, for 4.3 volt (USB supply) is appr. 90 to 100 - uint8_t DriveSpeed; // Speed PWM value used for going fixed distance. + uint8_t StartSpeedPWM; // SpeedPWM value at which car starts to move. For 8 volt is appr. 35 to 40, for 4.3 volt (USB supply) is appr. 90 to 100 + uint8_t DriveSpeedPWM; // SpeedPWM value used for going fixed distance. /* - * Positive value to be subtracted from TargetSpeed to get CurrentSpeed to compensate for different left and right motors - * Currently SpeedCompensation is in steps of 2 and only one motor can have a positive value, the other is set to zero. + * Positive value to be subtracted from TargetPWM to get CurrentSpeedPWM to compensate for different left and right motors + * Currently SpeedPWMCompensation is in steps of 2 and only one motor can have a positive value, the other is set to zero. * Value is computed in synchronizeMotor() */ - uint8_t SpeedCompensation; + uint8_t SpeedPWMCompensation; /********************************** * End of EEPROM values *********************************/ - uint8_t DefaultStopMode; // used for speed == 0 and STOP_MODE_KEEP - static bool MotorValuesHaveChanged; // true if DefaultStopMode, StartSpeed, DriveSpeed or SpeedCompensation have changed - for printing + uint8_t DefaultStopMode; // used for PWM == 0 and STOP_MODE_KEEP + static bool MotorControlValuesHaveChanged; // true if DefaultStopMode, StartSpeedPWM, DriveSpeedPWM or SpeedPWMCompensation have changed - for printing +#if defined(USE_MPU6050_IMU) || defined(USE_ENCODER_MOTOR_CONTROL) + volatile static bool SensorValuesHaveChanged; // true if encoder data or IMU data have changed +#endif - uint8_t CurrentSpeed; - uint8_t CurrentDirectionOrBrakeMode; // (of CurrentSpeed etc.) DIRECTION_FORWARD, DIRECTION_BACKWARD, MOTOR_BRAKE, MOTOR_RELEASE - static bool SpeedOrMotorModeHasChanged; + uint8_t CurrentSpeedPWM; + uint8_t CurrentDirectionOrBrakeMode; // (of CurrentSpeedPWM etc.) DIRECTION_FORWARD, DIRECTION_BACKWARD, MOTOR_BRAKE, MOTOR_RELEASE + uint8_t LastDirection; // Used for speed and distance. Contains DIRECTION_FORWARD, DIRECTION_BACKWARD but not MOTOR_BRAKE, MOTOR_RELEASE. + static bool MotorPWMHasChanged; - bool MotorMovesFixedDistance; // if true, stop if end distance condition is true + bool CheckDistanceInUpdateMotor; -#ifdef SUPPORT_RAMP_UP /* * For ramp control */ - uint8_t MotorRampState; // MOTOR_STATE_STOPPED, MOTOR_STATE_START, MOTOR_STATE_RAMP_UP, MOTOR_STATE_DRIVE_SPEED, MOTOR_STATE_RAMP_DOWN - uint8_t CurrentDriveSpeed; // DriveSpeed - SpeedCompensation; The DriveSpeed used for current movement. Can be set for eg. turning which better performs with reduced DriveSpeed + uint8_t MotorRampState; // MOTOR_STATE_STOPPED, MOTOR_STATE_START, MOTOR_STATE_RAMP_UP, MOTOR_STATE_DRIVE, MOTOR_STATE_RAMP_DOWN + uint8_t RequestedDriveSpeedPWM; // DriveSpeedPWM - SpeedPWMCompensation; The DriveSpeedPWM used for current movement. Can be set for eg. turning which better performs with reduced DriveSpeedPWM - uint8_t RampDelta; + uint8_t RampSpeedPWMDelta; // TODO is still constant, so remove? unsigned long NextRampChangeMillis; -#endif -#ifndef USE_ENCODER_MOTOR_CONTROL +#ifndef USE_ENCODER_MOTOR_CONTROL // this saves 5 bytes ram if we know, that we do not use the PWMDcMotor distance functions uint32_t computedMillisOfMotorStopForDistance; // Since we have no distance sensing, we must estimate a duration instead - unsigned int DistanceToTimeFactor; // Required for non encoder motors to estimate duration for a fixed distance + uint8_t MillisPerMillimeter; // Value for 2 volt motor effective voltage at DEFAULT_DRIVE_SPEED_PWM. Required for non encoder motors to estimate duration for a fixed distance #endif }; -//void PanicWithLed(unsigned int aDelay, uint8_t aCount); /* - * Version 1.1.0 - 10/2020 + * Version 2.0.0 - 11/2020 + * - IMU / MPU6050 support. + * - Support of off the shelf smart cars. * - Added and renamed functions. * * Version 1.0.0 - 9/2020 diff --git a/src/PathInfoPage.cpp b/src/PathInfoPage.cpp index 52acdb9..0dac202 100644 --- a/src/PathInfoPage.cpp +++ b/src/PathInfoPage.cpp @@ -37,10 +37,10 @@ void doResetPath(BDButton * aTheTouchedButton, int16_t aValue) { } void initPathInfoPage(void) { - TouchButtonResetPath.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR_RED, F("Clear"), TEXT_SIZE_22, + TouchButtonResetPath.init(0, 0, BUTTON_WIDTH_3_5, BUTTON_HEIGHT_6, COLOR16_RED, F("Clear"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doResetPath); - TouchButtonBackSmall.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR_RED, F("Back"), TEXT_SIZE_22, + TouchButtonBackSmall.init(BUTTON_WIDTH_4_POS_4, 0, BUTTON_WIDTH_4, BUTTON_HEIGHT_6, COLOR16_RED, F("Back"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &GUISwitchPages); } @@ -203,7 +203,7 @@ void DrawPath() { while (tXDeltaPtr <= sXPathDeltaPtr) { int tYDisplayDelta = (-(*tXDeltaPtr++)) >> tScaleShift; int tXDisplayDelta = (-(*tYDeltaPtr++)) >> tScaleShift; - BlueDisplay1.drawLineRel(tXDisplayPos, tYDisplayPos, tXDisplayDelta, tYDisplayDelta, COLOR_RED); + BlueDisplay1.drawLineRel(tXDisplayPos, tYDisplayPos, tXDisplayDelta, tYDisplayDelta, COLOR16_RED); tXDisplayPos += tXDisplayDelta; tYDisplayPos += tYDisplayDelta; } diff --git a/src/RobotCar.h b/src/RobotCar.h index c935f92..58859d9 100644 --- a/src/RobotCar.h +++ b/src/RobotCar.h @@ -20,14 +20,13 @@ #define SRC_ROBOTCAR_H_ #include -#include //#define CAR_HAS_4_WHEELS //#define USE_LAYOUT_FOR_NANO // Modify HC-SR04 by connecting 10kOhm between echo and trigger and then use only trigger. -//#define USE_US_SENSOR_1_PIN_MODE // Comment it out, if you use modified HC-SR04 modules or HY-SRF05 ones. +//#define USE_US_SENSOR_1_PIN_MODE // Activate it, if you use modified HC-SR04 modules or HY-SRF05 ones. //#define CAR_HAS_IR_DISTANCE_SENSOR @@ -49,8 +48,25 @@ */ // #define ENABLE_RTTTL // -#include "CarMotorControl.h" -extern CarMotorControl RobotCarMotorControl; +/* + * Shows VIN voltage and monitors it for undervoltage. VIN/11 at A2, 1MOhm to VIN, 100kOhm to ground + */ +//#define MONITOR_VIN_VOLTAGE +#if !defined(VIN_VOLTAGE_CORRECTION) +# ifdef ARDUINO_AVR_UNO +#define VIN_VOLTAGE_CORRECTION 0.8 +# endif +#endif +/* + * Activates the buttons to store compensation and drive speed + */ +//#define SUPPORT_EEPROM_STORAGE +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) +#include +#endif + +#include "CarPWMMotorControl.h" +extern CarPWMMotorControl RobotCarMotorControl; /* * Pin usage @@ -65,19 +81,19 @@ extern CarMotorControl RobotCarMotorControl; * 6 O Left motor PWM / NC for UNO board - connected to ENA * 7 O Right motor back / NC for UNO board - connected to IN3 * 8 O Left motor fwd / NC for UNO board - connected to IN2 - * 9 O Servo US distance - Servo Nr. 2 on Adafruit Motor Shield - * 10 O Servo laser pan - Servo Nr. 1 on Adafruit Motor Shield - * 11 O Servo laser tilt / Speaker for UNO board - * 12 O Left motor back / NC for UNO board - connected to IN1 + * 9 O Left motor back / NC for UNO board - connected to IN1 + * 10 O Servo US distance - Servo Nr. 2 on Adafruit Motor Shield + * 11 O Servo laser pan + * 12 O Servo laser tilt / Speaker for UNO board * 13 O Laser power * * A0 O US trigger (and echo in 1 pin US sensor mode) - * A1 I IR distance (needs 1 pin US sensor mode) / US echo - * A2 I VIN/11, 1MOhm to VIN, 100kOhm to ground. - * A3 IP NC + * A1 I US echo + * A2 I VIN/11, 1MOhm to VIN, 100kOhm to ground + * A3 I IR remote control signal in / IR distance / Speaker for Nano board * A4 SDA NC for Nano / I2C for UNO board motor shield * A5 SCL NC for Nano / I2C for UNO board motor shield - * A6 O Speaker for Nano board / not available on UNO board + * A6 O * A7 O Camera supply control */ @@ -91,24 +107,28 @@ extern CarMotorControl RobotCarMotorControl; #define PIN_RIGHT_MOTOR_BACKWARD 7 // IN3 #define PIN_RIGHT_MOTOR_PWM 5 // ENB - Must be PWM capable -#define PIN_LEFT_MOTOR_FORWARD 12 // IN1 - Pin 9 is already reserved for distance servo +#define PIN_LEFT_MOTOR_FORWARD 9 // IN1 #define PIN_LEFT_MOTOR_BACKWARD 8 // IN2 #define PIN_LEFT_MOTOR_PWM 6 // ENA - Must be PWM capable #endif +#ifdef USE_ENCODER_MOTOR_CONTROL +#define RIGHT_MOTOR_INTERRUPT INT0 // Pin 2 +#define LEFT_MOTOR_INTERRUPT INT1 // Pin 3 +#endif /* * Servo pins */ -#define PIN_DISTANCE_SERVO 9 // Servo Nr. 2 on Adafruit Motor Shield +#define PIN_DISTANCE_SERVO 10 // Servo Nr. 1 on Adafruit Motor Shield #ifdef CAR_HAS_PAN_SERVO -#define PIN_PAN_SERVO 10 // Servo Nr. 1 on Adafruit Motor Shield +#define PIN_PAN_SERVO 11 #endif #ifdef CAR_HAS_TILT_SERVO -#define PIN_TILT_SERVO 11 +#define PIN_TILT_SERVO 12 #endif -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) // Pin A0 for VCC monitoring - ADC channel 2 // Assume an attached resistor network of 100k / 10k from VCC to ground (divider by 11) #define VIN_11TH_IN_CHANNEL 2 // = A2 @@ -150,7 +170,7 @@ extern CarMotorControl RobotCarMotorControl; # ifdef CAR_HAS_CAMERA #define PIN_CAMERA_SUPPLY_CONTROL 4 # endif -#define PIN_BUZZER 11 +#define PIN_BUZZER 12 #endif /************************** @@ -180,20 +200,19 @@ extern Servo PanServo; extern Servo TiltServo; #endif - /************************************************************************************ * Definitions and declarations only used for GUI in RobotCarBlueDisplay.cpp example ************************************************************************************/ #define MINIMUM_DISTANCE_TO_SIDE 21 #define MINIMUM_DISTANCE_TO_FRONT 35 -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) #include "ADCUtils.h" extern float sVINVoltage; -#if defined(MONITOR_LIPO_VOLTAGE) -#define VOLTAGE_LOW_THRESHOLD 6.9 // Formula: 2 * 3.5 volt - voltage loss: 25 mV GND + 45 mV VIN + 35 mV Battery holder internal -#define VOLTAGE_USB_THRESHOLD 5.5 +#if defined(MONITOR_VIN_VOLTAGE) +#define VOLTAGE_LIPO_LOW_THRESHOLD 6.9 // Formula: 2 * 3.5 volt - voltage loss: 25 mV GND + 45 mV VIN + 35 mV Battery holder internal +#define VOLTAGE_USB_THRESHOLD 5.5 #else #endif #define VOLTAGE_TOO_LOW_DELAY_ONLINE 3000 // display VIN every 500 ms for 4 seconds diff --git a/src/RobotCarBlueDisplay.ino b/src/RobotCarBlueDisplay.ino index 7414110..cf574a3 100644 --- a/src/RobotCarBlueDisplay.ino +++ b/src/RobotCarBlueDisplay.ino @@ -39,12 +39,15 @@ #include #endif +//#define DEBUG_TRACE_INIT +//#include "AvrTracing.cpp.h" + /**************************************************************************** * Change this if you have reprogrammed the hc05 module for other baud rate ***************************************************************************/ #ifndef BLUETOOTH_BAUD_RATE -#define BLUETOOTH_BAUD_RATE BAUD_115200 -//#define BLUETOOTH_BAUD_RATE BAUD_9600 +//#define BLUETOOTH_BAUD_RATE BAUD_115200 +#define BLUETOOTH_BAUD_RATE BAUD_9600 #endif #define VERSION_EXAMPLE "3.0" @@ -52,7 +55,7 @@ /* * Car Control */ -CarMotorControl RobotCarMotorControl; +CarPWMMotorControl RobotCarMotorControl; float sVINVoltage; #ifdef ENABLE_RTTTL @@ -146,26 +149,33 @@ void setup() { tone(PIN_BUZZER, 2200, 50); // Booted - // initialize motors + initSerial(BLUETOOTH_BAUD_RATE); +// initTrace(); +// printNumberOfPushesForISR(); + + // initialize motors, this also stops motors #ifdef USE_ADAFRUIT_MOTOR_SHIELD - RobotCarMotorControl.init(true); // true -> read from EEPROM + RobotCarMotorControl.init(); #else # ifdef USE_ENCODER_MOTOR_CONTROL - RobotCarMotorControl.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM, PIN_LEFT_MOTOR_FORWARD, - PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, true); // true -> read from EEPROM + RobotCarMotorControl.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM, RIGHT_MOTOR_INTERRUPT, PIN_LEFT_MOTOR_FORWARD, + PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, LEFT_MOTOR_INTERRUPT); # else RobotCarMotorControl.init(PIN_RIGHT_MOTOR_FORWARD, PIN_RIGHT_MOTOR_BACKWARD, PIN_RIGHT_MOTOR_PWM, PIN_LEFT_MOTOR_FORWARD, - PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM, false); // false -> do NOT read from EEPROM + PIN_LEFT_MOTOR_BACKWARD, PIN_LEFT_MOTOR_PWM); # endif #endif - RobotCarMotorControl.stopMotors(); // in case motors were running before + +#ifdef SUPPORT_EEPROM_STORAGE + RobotCarMotorControl.readMotorValuesFromEeprom(); +#endif + + sRuningAutonomousDrive = false; delay(100); tone(PIN_BUZZER, 2200, 50); // motor initialized - sLastServoAngleInDegrees = 90; - - initSerial(BLUETOOTH_BAUD_RATE); + sLastServoAngleInDegrees = 90; // is required before setupGUI() // Must be after RobotCarMotorControl.init, since it tries to stop motors in connect callback setupGUI(); // this enables output by BlueDisplay1 and lasts around 100 milliseconds @@ -178,8 +188,8 @@ void setup() { // BlueDisplay1.debug("sMCUSR=", sMCUSR); } else { #if !defined(USE_SIMPLE_SERIAL) && !defined(USE_SERIAL1) // print it now if not printed above -#if defined(__AVR_ATmega32U4__) || defined(SERIAL_USB) || defined(SERIAL_PORT_USBVIRTUAL) - delay(2000); // To be able to connect Serial monitor after reset and before first printout +#if defined(__AVR_ATmega32U4__) || defined(SERIAL_USB) || defined(SERIAL_PORT_USBVIRTUAL) || defined(ARDUINO_attiny3217) + delay(4000); // To be able to connect Serial monitor after reset or power up and before first printout #endif // Just to know which program is running on my Arduino Serial.println(F("START " __FILE__ "\r\nVersion " VERSION_EXAMPLE " from " __DATE__)); @@ -200,18 +210,16 @@ void setup() { #endif initDistance(); -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) readVINVoltage(); randomSeed(sVINVoltage * 100); #endif -// initTrace(); - delay(100); if (sBootReasonWasPowerUp) { - tone(PIN_BUZZER, 2200, 50); // power up finished + tone(PIN_BUZZER, 1000, 50); // power up finished } else { - tone(PIN_BUZZER, 2200, 300); // long tone, reset finished - only detectable with Optiboot 8.0 + tone(PIN_BUZZER, 1000, 300); // long tone, reset finished - only detectable with Optiboot 8.0 } } @@ -222,10 +230,11 @@ void loop() { */ if ((!BlueDisplay1.isConnectionEstablished()) && (millis() < (TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS + 1000)) && (millis() > TIMOUT_BEFORE_DEMO_MODE_STARTS_MILLIS) -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) && (sVINVoltage > VOLTAGE_USB_THRESHOLD) #endif ) { + /* * Timeout just reached, play melody and start autonomous drive */ @@ -235,6 +244,7 @@ void loop() { #else delayAndLoopGUI(6000); // delay needed for millis() check above! #endif + // check again, maybe we are connected now if (!BlueDisplay1.isConnectionEstablished()) { // Set right page for reconnect GUISwitchPages(NULL, PAGE_AUTOMATIC_CONTROL); @@ -246,6 +256,10 @@ void loop() { } } + /* + * Required, if we use rotation, ramps and fixed distance driving + */ + RobotCarMotorControl.updateMotors(); /* * check for user input and update display output */ @@ -257,7 +271,11 @@ void loop() { if (BlueDisplay1.isConnectionEstablished() && sMillisOfLastReceivedBDEvent + TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS < millis()) { sMillisOfLastReceivedBDEvent = millis() - (TIMOUT_AFTER_LAST_BD_COMMAND_MILLIS / 2); // adjust sMillisOfLastReceivedBDEvent to have the next scan in 2 minutes fillAndShowForwardDistancesInfo(true, true); +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) DistanceServo.write(90); // set servo back to normal +#else + write10(90); +#endif } #ifdef ENABLE_RTTTL @@ -265,25 +283,12 @@ void loop() { * check for playing melody */ if (sPlayMelody) { - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); playRandomMelody(); } #endif - if (sCurrentPage == PAGE_TEST) { - /* - * Direct speed control by GUI - */ - if (RobotCarMotorControl.updateMotors()) { -#ifdef USE_ENCODER_MOTOR_CONTROL - // At least one motor is moving here - RobotCarMotorControl.rightCarMotor.synchronizeMotor(&RobotCarMotorControl.leftCarMotor, - MOTOR_DEFAULT_SYNCHRONIZE_INTERVAL_MILLIS); -#endif - } - } - if (sRuningAutonomousDrive) { if (sDriveMode == MODE_FOLLOWER) { driveFollowerModeOneStep(); @@ -293,21 +298,18 @@ void loop() { } } -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) void readVINVoltage() { - uint8_t tOldADMUX = checkAndWaitForReferenceAndChannelToSwitch(VIN_11TH_IN_CHANNEL, INTERNAL); - uint16_t tVIN = readADCChannelWithReferenceOversample(VIN_11TH_IN_CHANNEL, INTERNAL, 2); // 4 samples + uint16_t tVIN = readADCChannelWithReferenceOversampleFast(VIN_11TH_IN_CHANNEL, INTERNAL, 2); // 4 samples // BlueDisplay1.debug("VIN Raw=", tVIN); - // Switch back (to DEFAULT) - ADMUX = tOldADMUX; -// assume resistor network of 1Mk / 100k (divider by 11) -// tVCC * 0,01181640625 +// assume resistor network of 1MOhm / 100kOhm (divider by 11) +// tVIN * 0,01182795 #ifdef VIN_VOLTAGE_CORRECTION // we have a diode (requires 0.8 volt) between LIPO and VIN - sVINVoltage = (tVIN * ((11.0 * 1.07) / 1023)) + VIN_VOLTAGE_CORRECTION; + sVINVoltage = (tVIN * ((11.0 * 1.1) / 1023)) + VIN_VOLTAGE_CORRECTION; #else - sVINVoltage = tVIN * ((11.0 * 1.07) / 1023); + sVINVoltage = tVIN * ((11.0 * 1.1) / 1023); #endif } #endif @@ -323,7 +325,8 @@ void playRandomMelody() { // BlueDisplay1.debug("Play melody"); #if defined(USE_ADAFRUIT_MOTOR_SHIELD) - startPlayRandomRtttlFromArrayPGM(PIN_BUZZER, RTTTLMelodiesSmall, ARRAY_SIZE_MELODIES_SMALL); + startPlayRandomRtttlFromArrayPGM(PIN_BUZZER, RTTTLMelodiesTiny, ARRAY_SIZE_MELODIES_TINY); +// startPlayRandomRtttlFromArrayPGM(PIN_BUZZER, RTTTLMelodiesSmall, ARRAY_SIZE_MELODIES_SMALL); #else OCR2B = 0; bitWrite(TIMSK2, OCIE2B, 1); // enable interrupt for inverted pin handling @@ -391,6 +394,7 @@ Servo TiltServo; #endif void resetServos() { + sLastServoAngleInDegrees = 0; // to force setting of 90 degree DistanceServoWriteAndDelay(90, false); #ifdef CAR_HAS_PAN_SERVO PanServo.write(90); @@ -401,10 +405,16 @@ void resetServos() { } void initServos() { +#if defined(CAR_HAS_PAN_SERVO) || defined(CAR_HAS_TILT_SERVO) // DistanceServo.attach(PIN_DISTANCE_SERVO, DISTANCE_SERVO_MIN_PULSE_WIDTH, DISTANCE_SERVO_MAX_PULSE_WIDTH); DistanceServo.attach(PIN_DISTANCE_SERVO); +#else + // Servo for distance sensor + initLightweightServoPin10(); +#endif + #ifdef CAR_HAS_PAN_SERVO -// initialize and set Laser pan servo +// initialize and set laser pan servo PanServo.attach(PIN_PAN_SERVO); #endif #ifdef CAR_HAS_TILT_SERVO diff --git a/src/RobotCarCommonGui.cpp b/src/RobotCarCommonGui.cpp index 88c3004..a4e9405 100644 --- a/src/RobotCarCommonGui.cpp +++ b/src/RobotCarCommonGui.cpp @@ -27,6 +27,9 @@ #include "RobotCar.h" #include "RobotCarGui.h" #include "Distance.h" +#ifdef USE_MPU6050_IMU +#include "IMUCarData.h" +#endif // a string buffer for BD info output char sStringBuffer[128]; @@ -36,14 +39,14 @@ bool sSensorCallbacksEnabled; uint8_t sCurrentPage; BDButton TouchButtonBack; BDButton TouchButtonAutomaticDrivePage; -#ifdef USE_ENCODER_MOTOR_CONTROL -BDButton TouchButtonCalibrate; -#else -BDButton TouchButtonCompensation; -#endif BDButton TouchButtonCompensationRight; BDButton TouchButtonCompensationLeft; +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +BDButton TouchButtonCalibrate; +#endif +#ifdef SUPPORT_EEPROM_STORAGE BDButton TouchButtonCompensationStore; +#endif BDButton TouchButtonRobotCarStartStop; //bool sRobotCarMoving; // main start flag. If true motors are running and will be updated by main loop. @@ -68,8 +71,6 @@ BDSlider SliderIRDistance; unsigned int sSliderIRLastCentimeter; #endif -uint32_t sMillisOfNextVCCInfo = 0; - void setupGUI(void) { sCurrentPage = PAGE_HOME; @@ -86,15 +87,21 @@ void delayAndLoopGUI(uint16_t aDelayMillis) { } void loopGUI(void) { + /* + * Update IMU data first, they may be displayed later on + */ +#ifdef USE_MPU6050_IMU + // if we are going a fixed distance or are turning, updateTurnAngle() is yet called by updateMotors(), + // but updateIMUData has a timer to avoid multiple reading + RobotCarMotorControl.updateIMUData(); +#endif if (BlueDisplay1.isConnectionEstablished()) { - // do not show anything during motor speed ramps - if (!RobotCarMotorControl.isState(MOTOR_STATE_RAMP_UP)) { - /* - * All functions are empty until now - * actions are centrally managed below - */ + /* + * All functions are empty until now + * actions are centrally managed below + */ // if (sCurrentPage == PAGE_HOME) { // loopHomePage(); // } else if (sCurrentPage == PAGE_TEST) { @@ -112,32 +119,26 @@ void loopGUI(void) { // for all but PathInfo page if (sCurrentPage != PAGE_SHOW_PATH) { #endif -#if defined(MONITOR_LIPO_VOLTAGE) - readCheckAndPrintVinPeriodically(); -#endif - // For Home, sensorDrive and Test page - if (sCurrentPage != PAGE_AUTOMATIC_CONTROL) { - if (sCurrentPage != PAGE_BT_SENSOR_CONTROL) { - // For Home and Test page - readAndShowDistancePeriodically(DISTANCE_DISPLAY_PERIOD_MILLIS); - } - printMotorValues(); -#ifdef USE_ENCODER_MOTOR_CONTROL - displayVelocitySliderValues(); - /* - * Print changed tick values - */ - if (EncoderMotor::EncoderTickCounterHasChanged) { - EncoderMotor::EncoderTickCounterHasChanged = false; - printMotorDistanceValues(); - } +#if defined(MONITOR_VIN_VOLTAGE) + readCheckAndPrintVinPeriodically(); #endif + // For Home, sensorDrive and Test page + if (sCurrentPage != PAGE_AUTOMATIC_CONTROL) { + if (sCurrentPage != PAGE_BT_SENSOR_CONTROL) { + // For Home and Test page + readAndShowDistancePeriodically(); } +#if defined(USE_MPU6050_IMU) + printIMUOffsetValues(); +#endif + printMotorValuesPeriodically(); + } #ifdef ENABLE_PATH_INFO_PAGE } #endif - } + } + /* * Check if receive buffer contains an event * Do it at end since it may change the condition for calling this function and its printing @@ -156,16 +157,19 @@ void startStopRobotCar(bool aDoStart) { * Start for sensor drive */ sSensorChangeCallCountForZeroAdjustment = 0; - registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_UI, FLAG_SENSOR_NO_FILTER, + registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_NORMAL, FLAG_SENSOR_NO_FILTER, &doSensorChange); // Lock the screen orientation to avoid screen flip while rotating the smartphone BlueDisplay1.setScreenOrientationLock(FLAG_SCREEN_ORIENTATION_LOCK_CURRENT); sSensorCallbacksEnabled = true; - } else if (sLastSpeedSliderValue > 0) { + } else { + if (sLastSpeedSliderValue == 0) { + sLastSpeedSliderValue = RobotCarMotorControl.rightCarMotor.StartSpeedPWM; + } /* * Start car to last speed slider value */ - RobotCarMotorControl.setSpeedCompensated(sLastSpeedSliderValue, sRobotCarDirection); + RobotCarMotorControl.setSpeedPWMCompensated(sLastSpeedSliderValue, sRobotCarDirection); tSpeedSliderValue = sLastSpeedSliderValue; } } else { @@ -173,21 +177,21 @@ void startStopRobotCar(bool aDoStart) { * Stop car */ if (sRuningAutonomousDrive) { - startStopAutomomousDrive(false); + startStopAutomomousDrive(false, MODE_MANUAL_DRIVE); } if (sSensorCallbacksEnabled) { /* * Global stop for sensor drive */ - registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_UI, FLAG_SENSOR_NO_FILTER, NULL); + registerSensorChangeCallback(FLAG_SENSOR_TYPE_ACCELEROMETER, FLAG_SENSOR_DELAY_NORMAL, FLAG_SENSOR_NO_FILTER, NULL); BlueDisplay1.setScreenOrientationLock(FLAG_SCREEN_ORIENTATION_LOCK_UNLOCK); sSensorCallbacksEnabled = false; } - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); } bool tShowValues = sCurrentPage == PAGE_HOME || sCurrentPage == PAGE_TEST; - // set button value always just for the case we are called by another than Stop button callback +// set button value always just for the case we are called by another than Stop button callback TouchButtonRobotCarStartStop.setValue(!RobotCarMotorControl.isStopped() || sSensorCallbacksEnabled, tShowValues); if (tShowValues) { SliderSpeed.setValueAndDrawBar(tSpeedSliderValue); @@ -197,14 +201,22 @@ void startStopRobotCar(bool aDoStart) { #pragma GCC diagnostic ignored "-Wunused-parameter" -void doRobotCarStartStop(BDButton * aTheTouchedButton, int16_t aDoStart) { +void doStartStopRobotCar(BDButton *aTheTouchedButton, int16_t aDoStart) { startStopRobotCar(aDoStart); } -#ifdef USE_ENCODER_MOTOR_CONTROL -void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.calibrate(); - printMotorValues(); +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +void doCalibrate(BDButton *aTheTouchedButton, int16_t aValue) { + TouchButtonRobotCarStartStop.setValueAndDraw(RobotCarMotorControl.isStopped()); + if (RobotCarMotorControl.isStopped()) { + RobotCarMotorControl.calibrate(&loopGUI); +#ifdef SUPPORT_EEPROM_STORAGE + RobotCarMotorControl.writeMotorValuesToEeprom(); +#endif + } else { + // second / recursive call of doCalibrate() + RobotCarMotorControl.stop(); + } } #endif @@ -212,7 +224,7 @@ void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue) { * Minimum Speed is 30 for USB power and no load, 50 for load * Minimum Speed is 20 for 2 Lithium 18650 battery power and no load, 25 for load */ -void doSpeedSlider(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doSpeedSlider(BDSlider *aTheTouchedSlider, uint16_t aValue) { if (aValue != sLastSpeedSliderValue) { sLastSpeedSliderValue = aValue; @@ -220,64 +232,41 @@ void doSpeedSlider(BDSlider * aTheTouchedSlider, uint16_t aValue) { // handle GUI startStopRobotCar(true); } else { - RobotCarMotorControl.setSpeedCompensated(aValue, sRobotCarDirection); + RobotCarMotorControl.setSpeedPWMCompensated(aValue, sRobotCarDirection); } } } -void doUSServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue) { +void doUSServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue) { DistanceServoWriteAndDelay(aValue); } -#ifdef USE_ENCODER_MOTOR_CONTROL -/* - * Display velocity as slider values - */ -void displayVelocitySliderValues() { - EncoderMotor * tMotorInfo = &RobotCarMotorControl.leftCarMotor; - BDSlider * tSliderPtr = &SliderSpeedLeft; - uint16_t tXPos = 0; - for (int i = 0; i < 2; ++i) { - if (EncoderMotor::EncoderTickCounterHasChanged) { - tSliderPtr->setValueAndDrawBar(tMotorInfo->CurrentVelocity); - } - tMotorInfo = &RobotCarMotorControl.rightCarMotor; - tSliderPtr = &SliderSpeedRight; - tXPos += BUTTON_WIDTH_16 + 4; - } -} -#endif - /* * Stops car and change direction */ -void doSetDirection(BDButton * aTheTouchedButton, int16_t aValue) { +void doSetDirection(BDButton *aTheTouchedButton, int16_t aValue) { sRobotCarDirection = !aValue; // use inverse value since true is forward BUT 0 is DIRECTION_FORWARD // Stop fixed directions and turns using RobotCarMotorControl if (!RobotCarMotorControl.isStopped()) { - RobotCarMotorControl.stopMotors(); + RobotCarMotorControl.stop(); } // Stop direct movement by slider startStopRobotCar(false); } -void doStoreRightMotorSpeedCompensation(float aRightMotorSpeedCompensation) { - int8_t tRightMotorSpeedCompensation = aRightMotorSpeedCompensation; - RobotCarMotorControl.setValuesForFixedDistanceDriving(RobotCarMotorControl.rightCarMotor.StartSpeed, - RobotCarMotorControl.rightCarMotor.DriveSpeed, tRightMotorSpeedCompensation); -} - /* - * Request speed value as number + * changes speed compensation by +1 or -1 */ -void doSetCompensation(BDButton * aTheTouchedButton, int16_t aRightMotorSpeedCompensation) { - RobotCarMotorControl.setSpeedCompensation(aRightMotorSpeedCompensation); +void doSetCompensation(BDButton *aTheTouchedButton, int16_t aRightMotorSpeedPWMCompensation) { + RobotCarMotorControl.changeSpeedPWMCompensation(aRightMotorSpeedPWMCompensation); } -void doStoreCompensation(BDButton * aTheTouchedButton, int16_t aRightMotorSpeedCompensation) { - RobotCarMotorControl.writeMotorvaluesToEeprom(); +#ifdef SUPPORT_EEPROM_STORAGE +void doStoreCompensation(BDButton * aTheTouchedButton, int16_t aRightMotorSpeedPWMCompensation) { + RobotCarMotorControl.writeMotorValuesToEeprom(); } +#endif void startCurrentPage() { switch (sCurrentPage) { @@ -307,7 +296,7 @@ void startCurrentPage() { * Stop old page and start new one * @param aValue the Page number of the new page number */ -void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue) { +void GUISwitchPages(BDButton *aTheTouchedButton, int16_t aValue) { /* * Stop old page */ @@ -332,6 +321,9 @@ void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue) { #endif } +#ifdef USE_MPU6050_IMU + RobotCarMotorControl.IMUData.resetOffsetData(); // just to have a fresh start +#endif /* * Start new page */ @@ -340,9 +332,6 @@ void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue) { } void initRobotCarDisplay(void) { -// Stop any demo movement. - RobotCarMotorControl.stopMotors(); - sRuningAutonomousDrive = false; BlueDisplay1.setFlagsAndSize(BD_FLAG_FIRST_RESET_ALL | BD_FLAG_TOUCH_BASIC_DISABLE | BD_FLAG_USE_MAX_SIZE, DISPLAY_WIDTH, DISPLAY_HEIGHT); @@ -354,43 +343,45 @@ void initRobotCarDisplay(void) { /* * Common control buttons */ - TouchButtonRobotCarStartStop.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_BLUE, F("Start"), - TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doRobotCarStartStop); + TouchButtonRobotCarStartStop.init(0, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_BLUE, F("Start"), + TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, sRuningAutonomousDrive, &doStartStopRobotCar); TouchButtonRobotCarStartStop.setCaptionForValueTrue(F("Stop")); - TouchButtonBack.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR_RED, F("Back"), + TouchButtonBack.init(BUTTON_WIDTH_3_POS_3, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, COLOR16_RED, F("Back"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, PAGE_HOME, &GUISwitchPages); -#ifdef USE_ENCODER_MOTOR_CONTROL - TouchButtonCalibrate.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_RED, F("CAL"), +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + TouchButtonCalibrate.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_RED, F("CAL"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doCalibrate); #endif // Direction Button value true is forward, false is backward BUT 0 is DIRECTION_FORWARD!!! - TouchButtonDirection.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLACK, F("\x88"), + TouchButtonDirection.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLACK, F("\x88"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, true, &doSetDirection); TouchButtonDirection.setCaptionForValueTrue("\x87"); - TouchButtonCompensationLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, - BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR_BLUE, F("<-Co"), TEXT_SIZE_11, + TouchButtonCompensationLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4, + BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR16_BLUE, F("<-Co"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -1, &doSetCompensation); - TouchButtonCompensationRight.init(BUTTON_WIDTH_8_POS_5 - (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8_LINE_5, - BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR_BLUE, F("mp->"), TEXT_SIZE_11, + TouchButtonCompensationRight.init(BUTTON_WIDTH_8_POS_5 - (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8_LINE_4, + BUTTON_WIDTH_8 + (BUTTON_DEFAULT_SPACING_QUARTER - 1), BUTTON_HEIGHT_8, COLOR16_BLUE, F("mp->"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doSetCompensation); - TouchButtonCompensationStore.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, +#ifdef SUPPORT_EEPROM_STORAGE + TouchButtonCompensationStore.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("Store"), TEXT_SIZE_10, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 1, &doStoreCompensation); +#endif /* * Speed Sliders */ - SliderSpeed.init(0, SLIDER_TOP_MARGIN, BUTTON_WIDTH_6, SPEED_SLIDER_SIZE, 200, 0, COLOR_YELLOW, SLIDER_DEFAULT_BAR_COLOR, + SliderSpeed.init(0, SLIDER_TOP_MARGIN, BUTTON_WIDTH_6, SPEED_SLIDER_SIZE, 200, 0, COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doSpeedSlider); SliderSpeed.setScaleFactor(255.0 / SPEED_SLIDER_SIZE); // Slider is virtually 2 times larger than displayed, values were divided by 2 - SliderSpeedLeft.init(BUTTON_WIDTH_6 + 4, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, SPEED_SLIDER_SIZE / 2 - 1, 0, + SliderSpeedLeft.init(MOTOR_INFO_START_X, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, SPEED_SLIDER_SIZE / 2 - 1, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); SliderSpeedLeft.setValueFormatString("%3d"); // Since we also send values grater 100 - SliderSpeedRight.init(BUTTON_WIDTH_6 + 4 + BUTTON_WIDTH_16 + 8, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, + SliderSpeedRight.init(MOTOR_INFO_START_X + BUTTON_WIDTH_16 + 8, 0, BUTTON_WIDTH_16, SPEED_SLIDER_SIZE / 2, SPEED_SLIDER_SIZE / 2 - 1, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); SliderSpeedRight.setValueFormatString("%3d"); @@ -417,9 +408,9 @@ void initRobotCarDisplay(void) { #endif SliderUSPosition.init(POS_X_US_POSITION_SLIDER - BUTTON_WIDTH_6, SLIDER_TOP_MARGIN, BUTTON_WIDTH_6, US_SLIDER_SIZE, 90, 90, - COLOR_YELLOW, + COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doUSServoPosition); - SliderUSPosition.setBarThresholdColor(COLOR_BLUE); + SliderUSPosition.setBarThresholdColor(COLOR16_BLUE); SliderUSPosition.setScaleFactor(180.0 / US_SLIDER_SIZE); // Values from 0 to 180 degrees SliderUSPosition.setValueUnitString("\xB0"); // \xB0 is degree character @@ -433,22 +424,22 @@ void initRobotCarDisplay(void) { // Small US distance slider with captions and without cm units SliderUSDistance.init(POS_X_US_DISTANCE_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, (BUTTON_WIDTH_10 / 2) - 2, - DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, + DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER / DISTANCE_SLIDER_SCALE_FACTOR, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); - SliderUSDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR_BLACK, - COLOR_WHITE); + SliderUSDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR16_BLACK, + COLOR16_WHITE); SliderUSDistance.setCaption("US"); // below caption - left aligned SliderUSDistance.setPrintValueProperties(11, FLAG_SLIDER_CAPTION_ALIGN_LEFT | FLAG_SLIDER_CAPTION_BELOW, - 4 + TEXT_SIZE_10_HEIGHT, COLOR_BLACK, COLOR_WHITE); + 4 + TEXT_SIZE_10_HEIGHT, COLOR16_BLACK, COLOR16_WHITE); #else - // Big US distance slider without caption but with cm units POS_X_THIRD_SLIDER because it is the position of the left edge +// Big US distance slider without caption but with cm units POS_X_THIRD_SLIDER because it is the position of the left edge SliderUSDistance.init(POS_X_US_DISTANCE_SLIDER - BUTTON_WIDTH_10, SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, BUTTON_WIDTH_10, - DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, - FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); + DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER / DISTANCE_SLIDER_SCALE_FACTOR, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, + SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); SliderUSDistance.setValueUnitString("cm"); #endif - SliderUSDistance.setScaleFactor(2); // Slider is virtually 2 times larger, values were divided by 2 + SliderUSDistance.setScaleFactor(DISTANCE_SLIDER_SCALE_FACTOR); // Slider is virtually 2 times larger, values were divided by 2 SliderUSDistance.setBarThresholdColor(DISTANCE_TIMEOUT_COLOR); /* @@ -458,25 +449,25 @@ void initRobotCarDisplay(void) { // Small IR distance slider with captions and without cm units SliderIRDistance.init(POS_X_THIRD_SLIDER - ((BUTTON_WIDTH_10 / 2) - 2), SLIDER_TOP_MARGIN + BUTTON_HEIGHT_8, (BUTTON_WIDTH_10 / 2) - 2, - DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, + DISTANCE_SLIDER_SIZE, DISTANCE_TIMEOUT_CM_FOLLOWER / DISTANCE_SLIDER_SCALE_FACTOR, 0, SLIDER_DEFAULT_BACKGROUND_COLOR, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE | FLAG_SLIDER_IS_ONLY_OUTPUT, NULL); - SliderIRDistance.setScaleFactor(2); // Slider is virtually 2 times larger, values were divided by 2 + SliderIRDistance.setScaleFactor(DISTANCE_SLIDER_SCALE_FACTOR); // Slider is virtually 2 times larger, values were divided by 2 SliderIRDistance.setBarThresholdColor(DISTANCE_TIMEOUT_COLOR); // Caption properties - SliderIRDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_RIGHT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR_BLACK, - COLOR_WHITE); + SliderIRDistance.setCaptionProperties(TEXT_SIZE_10, FLAG_SLIDER_CAPTION_ALIGN_RIGHT | FLAG_SLIDER_CAPTION_BELOW, 2, COLOR16_BLACK, + COLOR16_WHITE); // Captions SliderIRDistance.setCaption("IR"); // value below caption - right aligned SliderIRDistance.setPrintValueProperties(11, FLAG_SLIDER_CAPTION_ALIGN_RIGHT | FLAG_SLIDER_CAPTION_BELOW, - 4 + TEXT_SIZE_10_HEIGHT, COLOR_BLACK, COLOR_WHITE); + 4 + TEXT_SIZE_10_HEIGHT, COLOR16_BLACK, COLOR16_WHITE); #endif #ifdef CAR_HAS_PAN_SERVO // left of SliderUSPosition - SliderPan.init(POS_X_PAN_SLIDER - BUTTON_WIDTH_12, SLIDER_TOP_MARGIN, BUTTON_WIDTH_12, LASER_SLIDER_SIZE, 90, 90, COLOR_YELLOW, + SliderPan.init(POS_X_PAN_SLIDER - BUTTON_WIDTH_12, SLIDER_TOP_MARGIN, BUTTON_WIDTH_12, LASER_SLIDER_SIZE, 90, 90, COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doHorizontalServoPosition); - SliderPan.setBarThresholdColor(COLOR_BLUE); + SliderPan.setBarThresholdColor(COLOR16_BLUE); // scale slider values SliderPan.setScaleFactor(180.0 / LASER_SLIDER_SIZE); // Values from 0 to 180 degrees SliderPan.setValueUnitString("\xB0"); // \xB0 is degree character @@ -484,9 +475,8 @@ void initRobotCarDisplay(void) { #ifdef CAR_HAS_TILT_SERVO SliderTilt.init(POS_X_TILT_SLIDER - BUTTON_WIDTH_12, SLIDER_TOP_MARGIN, BUTTON_WIDTH_12, LASER_SLIDER_SIZE, 90, - TILT_SERVO_MIN_VALUE, COLOR_YELLOW, - SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doVerticalServoPosition); - SliderTilt.setBarThresholdColor(COLOR_BLUE); + TILT_SERVO_MIN_VALUE, COLOR16_YELLOW, SLIDER_DEFAULT_BAR_COLOR, FLAG_SLIDER_SHOW_VALUE, &doVerticalServoPosition); + SliderTilt.setBarThresholdColor(COLOR16_BLUE); // scale slider values SliderTilt.setScaleFactor(180.0 / LASER_SLIDER_SIZE); // Values from 0 to 180 degrees SliderTilt.setValueUnitString("\xB0"); // \xB0 is degree character @@ -504,18 +494,18 @@ void initRobotCarDisplay(void) { void drawCommonGui(void) { clearDisplayAndDisableButtonsAndSliders(); - BlueDisplay1.drawText(HEADER_X, TEXT_SIZE_22_HEIGHT, F("Robot Car"), TEXT_SIZE_22, COLOR_BLUE, - COLOR_NO_BACKGROUND); + BlueDisplay1.drawText(HEADER_X, TEXT_SIZE_22_HEIGHT, F("Robot Car"), TEXT_SIZE_22, COLOR16_BLUE, + COLOR16_NO_BACKGROUND); } -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) /* * Print VIN (used as motor supply) periodically * returns true if voltage was printed */ void readAndPrintVin() { char tDataBuffer[18]; - char tVCCString[6]; + char tVCCString[5]; readVINVoltage(); dtostrf(sVINVoltage, 4, 2, tVCCString); sprintf_P(tDataBuffer, PSTR("%s volt"), tVCCString); @@ -534,24 +524,24 @@ void readAndPrintVin() { tPosX = BUTTON_WIDTH_3_POS_3 - BUTTON_DEFAULT_SPACING - (8 * TEXT_SIZE_11_WIDTH); tPosY = BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND; } - BlueDisplay1.drawText(tPosX, tPosY, tDataBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); + BlueDisplay1.drawText(tPosX, tPosY, tDataBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); } void checkForLowVoltage() { static uint8_t sLowVoltageCount = 0; - if (sVINVoltage < VOLTAGE_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD) { + if (sVINVoltage < VOLTAGE_LIPO_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD) { sLowVoltageCount++; } else if (sLowVoltageCount > 1) { sLowVoltageCount--; } if (sLowVoltageCount > 2) { // Here more than 2 consecutive times (for 6 seconds) low voltage detected - RobotCarMotorControl.stopMotors(); + startStopRobotCar(false); if (BlueDisplay1.isConnectionEstablished()) { drawCommonGui(); - BlueDisplay1.drawText(10, 50, F("Battery voltage"), TEXT_SIZE_33, COLOR_RED, COLOR_WHITE); + BlueDisplay1.drawText(10, 50, F("Battery voltage"), TEXT_SIZE_33, COLOR16_RED, COLOR16_WHITE); // print current "too low" voltage char tDataBuffer[18]; char tVCCString[6]; @@ -572,7 +562,7 @@ void checkForLowVoltage() { delayMillisWithCheckAndHandleEvents(500); // and wait tLoopCount--; readVINVoltage(); // read new VCC value - } while (tLoopCount > 0 || (sVINVoltage < VOLTAGE_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD)); + } while (tLoopCount > 0 || (sVINVoltage < VOLTAGE_LIPO_LOW_THRESHOLD && sVINVoltage > VOLTAGE_USB_THRESHOLD)); // refresh current page GUISwitchPages(NULL, 0); } else { @@ -582,53 +572,154 @@ void checkForLowVoltage() { } void readCheckAndPrintVinPeriodically() { + static uint32_t sMillisOfLastVCCInfo; uint32_t tMillis = millis(); - if (tMillis >= sMillisOfNextVCCInfo) { - sMillisOfNextVCCInfo = tMillis + PRINT_VOLTAGE_PERIOD_MILLIS; + if (tMillis - sMillisOfLastVCCInfo >= PRINT_VOLTAGE_PERIOD_MILLIS) { + sMillisOfLastVCCInfo = tMillis; readAndPrintVin(); checkForLowVoltage(); + PWMDcMotor::MotorControlValuesHaveChanged = true; // to signal, that PWM voltages may have changed } } -#endif - -void printMotorSpeed() { - if (PWMDcMotor::SpeedOrMotorModeHasChanged) { - PWMDcMotor::SpeedOrMotorModeHasChanged = false; -// position below caption of speed slider - uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + TEXT_SIZE_11_HEIGHT; - sprintf_P(sStringBuffer, PSTR("speed%3d %3d"), RobotCarMotorControl.leftCarMotor.CurrentSpeed, - RobotCarMotorControl.rightCarMotor.CurrentSpeed); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); - } -} +#endif // defined(MONITOR_VIN_VOLTAGE) -void printMotorValues() { - printMotorSpeed(); - if (PWMDcMotor::MotorValuesHaveChanged) { - PWMDcMotor::MotorValuesHaveChanged = false; -// position below speed values - uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + (2 * TEXT_SIZE_11_HEIGHT); - sprintf_P(sStringBuffer, PSTR("comp. %2d %2d"), RobotCarMotorControl.leftCarMotor.SpeedCompensation, - RobotCarMotorControl.rightCarMotor.SpeedCompensation); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); +/* + * Checks MotorControlValuesHaveChanged and print values + */ +void printMotorValuesPeriodically() { + static long sLastPrintMillis; - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("min. %3d %3d"), RobotCarMotorControl.leftCarMotor.StartSpeed, - RobotCarMotorControl.rightCarMotor.StartSpeed); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); + uint32_t tMillis = millis(); + if (tMillis - sLastPrintMillis >= PRINT_MOTOR_INFO_PERIOD_MILLIS) { + sLastPrintMillis = tMillis; + /* + * Print speed value + */ + if (PWMDcMotor::MotorPWMHasChanged) { + PWMDcMotor::MotorPWMHasChanged = false; + // position below caption of speed slider + uint16_t tYPos = MOTOR_INFO_START_Y + TEXT_SIZE_11; + sprintf_P(sStringBuffer, PSTR("PWM %3d %3d"), RobotCarMotorControl.leftCarMotor.CurrentSpeedPWM, + RobotCarMotorControl.rightCarMotor.CurrentSpeedPWM); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); + } + /* + * Print motor control values + */ + if (PWMDcMotor::MotorControlValuesHaveChanged) { + PWMDcMotor::MotorControlValuesHaveChanged = false; + // position below speed values + uint16_t tYPos = MOTOR_INFO_START_Y + (2 * TEXT_SIZE_11); + sprintf_P(sStringBuffer, PSTR("lcomp %2d"), + RobotCarMotorControl.rightCarMotor.SpeedPWMCompensation + - RobotCarMotorControl.leftCarMotor.SpeedPWMCompensation); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); + + tYPos += TEXT_SIZE_11; + char tVCCString[5]; +#ifdef MONITOR_VIN_VOLTAGE + dtostrf( + (((float) RobotCarMotorControl.rightCarMotor.StartSpeedPWM * sVINVoltage) + - (FULL_BRIDGE_LOSS_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#else + dtostrf( + (RobotCarMotorControl.rightCarMotor.StartSpeedPWM * (FULL_BRIDGE_OUTPUT_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#endif + sprintf_P(sStringBuffer, PSTR("st.%3d %sV"), RobotCarMotorControl.rightCarMotor.StartSpeedPWM, tVCCString); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + + tYPos += TEXT_SIZE_11; +#ifdef MONITOR_VIN_VOLTAGE + dtostrf( + (((float) RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * sVINVoltage) + - (FULL_BRIDGE_LOSS_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#else + dtostrf( + (RobotCarMotorControl.rightCarMotor.DriveSpeedPWM * (FULL_BRIDGE_OUTPUT_MILLIVOLT / 1000.0)) / MAX_SPEED_PWM, 4, 2, tVCCString); +#endif + sprintf_P(sStringBuffer, PSTR("drv%3d %sV"), RobotCarMotorControl.rightCarMotor.DriveSpeedPWM, tVCCString); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("max. %3d %3d"), RobotCarMotorControl.leftCarMotor.DriveSpeed, - RobotCarMotorControl.rightCarMotor.DriveSpeed); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); + } #ifdef USE_ENCODER_MOTOR_CONTROL if (sShowDebug && sCurrentPage == PAGE_TEST) { printMotorDebugValues(); } #endif +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + printMotorSensorValues(); +#endif + } +} + +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +/* + * Display speed as slider values encoder values have precedence over IMU values + */ +void displayMotorSpeedSliderValues() { +# if defined(USE_ENCODER_MOTOR_CONTROL) + EncoderMotor *tMotorInfo = &RobotCarMotorControl.leftCarMotor; + BDSlider *tSliderPtr = &SliderSpeedLeft; + for (int i = 0; i < 2; ++i) { + tSliderPtr->setValueAndDrawBar(tMotorInfo->getSpeed()); + tMotorInfo = &RobotCarMotorControl.rightCarMotor; + tSliderPtr = &SliderSpeedRight; + } +# else + int tCarSpeedCmPerSecondFromIMU = abs(RobotCarMotorControl.CarSpeedCmPerSecondFromIMU); + SliderSpeedLeft.setValueAndDrawBar(tCarSpeedCmPerSecondFromIMU); + SliderSpeedRight.setValueAndDrawBar(tCarSpeedCmPerSecondFromIMU); +# endif +} + +/* + * Encoder speed has precedence over IMU speed + */ +void printMotorSensorValues() { + if (PWMDcMotor::SensorValuesHaveChanged) { + PWMDcMotor::SensorValuesHaveChanged = false; + +# if defined(USE_ENCODER_MOTOR_CONTROL) + /* + * Print encoder counts + */ + uint16_t tYPos; + if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { + tYPos = BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND; + } else { + tYPos = MOTOR_INFO_START_Y; + } + sprintf_P(sStringBuffer, PSTR("cnt.%4d%4d"), RobotCarMotorControl.leftCarMotor.EncoderCount, + RobotCarMotorControl.rightCarMotor.EncoderCount); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, COLOR16_WHITE); +# endif + +# if defined(USE_MPU6050_IMU) + /* + * Print distance and rotation from IMU + */ + sprintf_P(sStringBuffer, PSTR("%5dcm%4d\xB0"), RobotCarMotorControl.IMUData.getDistanceCm(), + RobotCarMotorControl.CarTurnAngleHalfDegreesFromIMU / 2); + BlueDisplay1.drawText(MOTOR_INFO_START_X, MOTOR_INFO_START_Y, sStringBuffer); +# endif + + displayMotorSpeedSliderValues(); + } +} +#endif // defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) + +#if defined(USE_MPU6050_IMU) +void printIMUOffsetValues() { + if (RobotCarMotorControl.IMUData.OffsetsHaveChanged) { + RobotCarMotorControl.IMUData.OffsetsHaveChanged = false; + sprintf_P(sStringBuffer, PSTR("off.%4d%4d"), RobotCarMotorControl.IMUData.AcceleratorForwardOffset, + RobotCarMotorControl.IMUData.GyroscopePanOffset); + BlueDisplay1.drawText(MOTOR_INFO_START_X, MOTOR_INFO_START_Y + (5 * TEXT_SIZE_11), sStringBuffer, TEXT_SIZE_11, COLOR16_BLACK, + COLOR16_WHITE); } } +#endif // defined(USE_MPU6050_IMU) #ifdef USE_ENCODER_MOTOR_CONTROL /* @@ -638,45 +729,41 @@ void printMotorDebugValues() { /* * Debug info */ - uint16_t tYPos = SPEED_SLIDER_SIZE / 2 + 25 + TEXT_SIZE_11_HEIGHT + (4 * TEXT_SIZE_11); -#ifdef SUPPORT_RAMP_UP - sprintf_P(sStringBuffer, PSTR("ramp1%3d %3d"), RobotCarMotorControl.leftCarMotor.DistanceCountAfterRampUp, - RobotCarMotorControl.rightCarMotor.DistanceCountAfterRampUp); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("endSp%3d %3d"), RobotCarMotorControl.leftCarMotor.DebugSpeedAtTargetCountReached, - RobotCarMotorControl.rightCarMotor.DebugSpeedAtTargetCountReached); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("dcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.DebugCount, - RobotCarMotorControl.rightCarMotor.DebugCount); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - tYPos += TEXT_SIZE_11; -#endif - sprintf_P(sStringBuffer, PSTR("debug%3d %3d"), RobotCarMotorControl.leftCarMotor.Debug, - RobotCarMotorControl.rightCarMotor.Debug); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); - - tYPos += TEXT_SIZE_11; - sprintf_P(sStringBuffer, PSTR("tcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.LastTargetDistanceCount, - RobotCarMotorControl.rightCarMotor.LastTargetDistanceCount); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer); -} +// if (EncoderMotor::EncoderRampUpDataHaveChanged) { +// EncoderMotor::EncoderRampUpDataHaveChanged = false; -void printMotorDistanceValues() { - uint16_t tYPos; - if (sCurrentPage == PAGE_AUTOMATIC_CONTROL) { - tYPos = BUTTON_HEIGHT_4_LINE_4 - TEXT_SIZE_11_DECEND; - } else { - tYPos = SPEED_SLIDER_SIZE / 2 + 25; - } - sprintf_P(sStringBuffer, PSTR("cnt.%4d%4d"), RobotCarMotorControl.leftCarMotor.EncoderCount, - RobotCarMotorControl.rightCarMotor.EncoderCount); - BlueDisplay1.drawText(BUTTON_WIDTH_6 + 4, tYPos, sStringBuffer, TEXT_SIZE_11, COLOR_BLACK, COLOR_WHITE); +# if defined(USE_MPU6050_IMU) + uint16_t tYPos = MOTOR_INFO_START_Y + (6 * TEXT_SIZE_11); +# else + uint16_t tYPos = MOTOR_INFO_START_Y + (5 * TEXT_SIZE_11); +# endif + +# ifdef SUPPORT_RAMP_UP + sprintf_P(sStringBuffer, PSTR("ramp1%3d %3d"), RobotCarMotorControl.leftCarMotor.DistanceCountAfterRampUp, + RobotCarMotorControl.rightCarMotor.DistanceCountAfterRampUp); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + + tYPos += TEXT_SIZE_11; + sprintf_P(sStringBuffer, PSTR("endSp%3d %3d"), RobotCarMotorControl.leftCarMotor.DebugSpeedAtTargetCountReached, + RobotCarMotorControl.rightCarMotor.DebugSpeedAtTargetCountReached); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + + tYPos += TEXT_SIZE_11; +# endif + + sprintf_P(sStringBuffer, PSTR("tcnt %3d %3d"), RobotCarMotorControl.leftCarMotor.LastTargetDistanceMillimeter, + RobotCarMotorControl.rightCarMotor.LastTargetDistanceMillimeter); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); + +# if !defined(USE_MPU6050_IMU) // no space if IMU data is displayed + tYPos += TEXT_SIZE_11; + sprintf_P(sStringBuffer, PSTR("debug%3d %3d"), RobotCarMotorControl.leftCarMotor.Debug, + RobotCarMotorControl.rightCarMotor.Debug); + BlueDisplay1.drawText(MOTOR_INFO_START_X, tYPos, sStringBuffer); +# endif +// } } -#endif +#endif // USE_ENCODER_MOTOR_CONTROL void showUSDistance(unsigned int aCentimeter, bool aForceDraw) { // feedback as slider length diff --git a/src/RobotCarGui.h b/src/RobotCarGui.h index 24783c7..9e31eed 100644 --- a/src/RobotCarGui.h +++ b/src/RobotCarGui.h @@ -31,7 +31,7 @@ #define PATH_LENGTH_MAX 100 #define PRINT_VOLTAGE_PERIOD_MILLIS 2000 -extern uint32_t sMillisOfNextVCCInfo; +#define PRINT_MOTOR_INFO_PERIOD_MILLIS 200 // a string buffer for BD info output extern char sStringBuffer[128]; @@ -46,10 +46,14 @@ extern char sStringBuffer[128]; #define US_SLIDER_SIZE BUTTON_HEIGHT_4_LINE_3 // 128 #define LASER_SLIDER_SIZE BUTTON_HEIGHT_4_LINE_3 // 128 #define DISTANCE_SLIDER_SIZE (BUTTON_HEIGHT_4_LINE_3 - BUTTON_HEIGHT_8) // 104 +#define DISTANCE_SLIDER_SCALE_FACTOR 2 // Slider is virtually 2 times larger, values were divided by 2 #define US_DISTANCE_MAP_ORIGIN_X 200 #define US_DISTANCE_MAP_ORIGIN_Y 150 +#define MOTOR_INFO_START_X (BUTTON_WIDTH_6 + 4) +#define MOTOR_INFO_START_Y (SPEED_SLIDER_SIZE / 2 + 26) + #define PAGE_HOME 0 // Manual control page #define PAGE_AUTOMATIC_CONTROL 1 #define PAGE_BT_SENSOR_CONTROL 2 @@ -88,11 +92,11 @@ void loopAutonomousDrivePage(void); void stopAutonomousDrivePage(void); void handleAutomomousDriveRadioButtons(); -void doStartStopFollowerMode(BDButton * aTheTouchedButton, int16_t aValue); -void doStartStopAutomomousDrive(BDButton * aTheTouchedButton, int16_t aValue); -void doStartStopTestUser(BDButton * aTheTouchedButton, int16_t aValue); +void doStartStopFollowerMode(BDButton *aTheTouchedButton, int16_t aValue); +void doStartStopAutomomousDrive(BDButton *aTheTouchedButton, int16_t aValue); +void doStartStopTestUser(BDButton *aTheTouchedButton, int16_t aValue); -void doStartStopAutonomousForPathPage(BDButton * aTheTouchedButton, int16_t aValue); +void doStartStopAutonomousForPathPage(BDButton *aTheTouchedButton, int16_t aValue); void setStepMode(uint8_t aStepMode); // from BTSensorDrivePage @@ -103,7 +107,7 @@ void loopBTSensorDrivePage(void); void stopBTSensorDrivePage(void); extern uint8_t sSensorChangeCallCountForZeroAdjustment; -void doSensorChange(uint8_t aSensorType, struct SensorCallback * aSensorCallbackInfo); +void doSensorChange(uint8_t aSensorType, struct SensorCallback *aSensorCallbackInfo); // from TestPage extern bool sShowDebug; @@ -120,8 +124,8 @@ extern BDButton TouchButtonMelody; extern bool sPlayMelody; #endif -extern void doHorizontalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue); -extern void doVerticalServoPosition(BDSlider * aTheTouchedSlider, uint16_t aValue); +extern void doHorizontalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue); +extern void doVerticalServoPosition(BDSlider *aTheTouchedSlider, uint16_t aValue); void initHomePage(void); void drawHomePage(void); @@ -136,7 +140,7 @@ extern uint8_t sCurrentPage; extern BDButton TouchButtonAutomaticDrivePage; extern BDButton TouchButtonReset; extern BDButton TouchButtonBack; -void GUISwitchPages(BDButton * aTheTouchedButton, int16_t aValue); +void GUISwitchPages(BDButton *aTheTouchedButton, int16_t aValue); void startCurrentPage(); /* @@ -145,17 +149,20 @@ void startCurrentPage(); extern BDButton TouchButtonRobotCarStartStop; void setStartStopButtonValue(); void startStopRobotCar(bool aDoStart); -void doRobotCarStartStop(BDButton * aTheTochedButton, int16_t aDoStart); +void doStartStopRobotCar(BDButton *aTheTochedButton, int16_t aDoStart); +void doReset(BDButton *aTheTochedButton, int16_t aValue); extern BDButton TouchButtonDirection; -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) extern BDButton TouchButtonCalibrate; -void doCalibrate(BDButton * aTheTouchedButton, int16_t aValue); +void doCalibrate(BDButton *aTheTouchedButton, int16_t aValue); #endif extern BDButton TouchButtonCompensationRight; extern BDButton TouchButtonCompensationLeft; +#ifdef SUPPORT_EEPROM_STORAGE extern BDButton TouchButtonCompensationStore; +#endif extern BDSlider SliderSpeed; extern uint16_t sLastSpeedSliderValue; @@ -177,8 +184,12 @@ extern BDSlider SliderPan; extern BDSlider SliderTilt; #endif -#ifdef USE_ENCODER_MOTOR_CONTROL -void displayVelocitySliderValues(); +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) +void displayMotorSpeedSliderValues(); +void printMotorSensorValues(); +#endif +#if defined(USE_MPU6050_IMU) +void printIMUOffsetValues(); #endif void drawCommonGui(void); @@ -189,18 +200,18 @@ void setupGUI(void); void loopGUI(void); void initRobotCarDisplay(void); -void readAndShowDistancePeriodically(uint16_t aPeriodMillis); +void readAndShowDistancePeriodically(); void rotate(int16_t aRotationDegrees, bool inPlace = true); void showDistance(int aCentimeter); -void printMotorSpeed(); -void printMotorValues(); +void printAndDisplayMotorSpeed(); +void printMotorValuesPeriodically(); #ifdef USE_ENCODER_MOTOR_CONTROL void printMotorDebugValues(); void printMotorDistanceValues(); #endif -#if defined(MONITOR_LIPO_VOLTAGE) +#if defined(MONITOR_VIN_VOLTAGE) void readAndPrintVin(); void readCheckAndPrintVinPeriodically(); #endif diff --git a/src/TestPage.cpp b/src/TestPage.cpp index bf57cc6..e2c590e 100644 --- a/src/TestPage.cpp +++ b/src/TestPage.cpp @@ -28,7 +28,9 @@ */ BDButton TouchButtonReset; +#ifdef SUPPORT_EEPROM_STORAGE BDButton TouchButtonGetAndStoreSpeed; +#endif BDButton TouchButtonDebug; @@ -46,18 +48,37 @@ BDButton TouchButton360Degree; bool sShowDebug = false; #pragma GCC diagnostic ignored "-Wunused-parameter" -void doDistance(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.startGoDistanceCentimeter(aValue, sRobotCarDirection); +void doDistance(BDButton *aTheTouchedButton, int16_t aValue) { + RobotCarMotorControl.startGoDistanceMillimeter(aValue, sRobotCarDirection); } -void doShowDebug(BDButton * aTheTouchedButton, int16_t aValue) { +void doShowDebug(BDButton *aTheTouchedButton, int16_t aValue) { sShowDebug = aValue; } -void doRotation(BDButton * aTheTouchedButton, int16_t aValue) { - RobotCarMotorControl.startRotateCar(aValue, sRobotCarDirection, true); +/* + * stop and reset motors + * reset IMU data and compute new offsets + */ +void doReset(BDButton *aTheTouchedButton, int16_t aValue) { + startStopRobotCar(false); + RobotCarMotorControl.resetControlValues(); + sLastSpeedSliderValue = 0; } +void doRotation(BDButton *aTheTouchedButton, int16_t aValue) { + if (aValue == 360) { + // use in place for 360 degree and change turn direction according to sRobotCarDirection + if (sRobotCarDirection != DIRECTION_FORWARD) { + aValue = -aValue; + } + RobotCarMotorControl.startRotate(aValue, TURN_IN_PLACE); + } else { + RobotCarMotorControl.startRotate(aValue, sRobotCarDirection); + } +} + +#ifdef SUPPORT_EEPROM_STORAGE /* * Callback handler for user speed input * Store user speed input as DriveSpeed @@ -69,64 +90,108 @@ void doStoreSpeed(float aValue) { RobotCarMotorControl.rightCarMotor.DriveSpeed = tValue; // use the same value here ! RobotCarMotorControl.leftCarMotor.DriveSpeed = tValue; - RobotCarMotorControl.writeMotorvaluesToEeprom(); + RobotCarMotorControl.writeMotorValuesToEeprom(); } printMotorValues(); } + /* * Request speed value as number from user */ void doGetSpeedAsNumber(BDButton * aTheTouchedButton, int16_t aValue) { BlueDisplay1.getNumberWithShortPrompt(&doStoreSpeed, "Drive speed [11-255]", sLastSpeedSliderValue); } +#endif + +//const struct ButtonInit ButtonReset PROGMEM { BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, +//COLOR16_BLUE, TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doReset }; +// +//const struct ButtonInit Button5cm PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 50, &doDistance }; +//const struct ButtonInit Button10cm PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 100, &doDistance }; +// +//const struct ButtonInit Button20cm PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 200, &doDistance }; +//const struct ButtonInit Button40cm PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 400, &doDistance }; +//const struct ButtonInit ButtonDebug PROGMEM { BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_RED, TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doShowDebug }; +// +//const struct ButtonInit Button45DegreeLeft PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 45, &doRotation }; // \xB0 is degree character +//const struct ButtonInit Button45DegreeRight PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -45, &doRotation }; // \xB0 is degree character +//const struct ButtonInit Button360Degree PROGMEM { BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 360, &doRotation }; // \xB0 is degree character +// +//const struct ButtonInit Button90DegreeLeft PROGMEM { BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 90, &doRotation }; // \xB0 is degree character +//const struct ButtonInit Button90DegreeRight PROGMEM { BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, +//COLOR16_BLUE, +//TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -90, &doRotation }; // \xB0 is degree character /* - * stop and reset motors + * replacing parameter init with structure init INCREASES code size by 82 bytes */ -void doReset(BDButton * aTheTouchedButton, int16_t aValue) { - startStopRobotCar(false); - RobotCarMotorControl.resetControlValues(); -} - void initTestPage(void) { /* * Control buttons */ - TouchButtonReset.init(BUTTON_WIDTH_3_POS_2, BUTTON_HEIGHT_4_LINE_4, BUTTON_WIDTH_3, BUTTON_HEIGHT_4, - COLOR_BLUE, F("Reset"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doReset); + COLOR16_BLUE, F("Reset"), TEXT_SIZE_22, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doReset); +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonGetAndStoreSpeed.init(0, BUTTON_HEIGHT_4_LINE_4 - BUTTON_HEIGHT_6 - BUTTON_DEFAULT_SPACING_QUARTER, BUTTON_WIDTH_6, - BUTTON_HEIGHT_6, COLOR_BLUE, F("Set\nspeed"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doGetSpeedAsNumber); - + BUTTON_HEIGHT_6, COLOR16_BLUE, F("Set\nspeed"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 0, &doGetSpeedAsNumber); +#endif /* * Test buttons * Many calls requires 36 bytes code + sometimes 52 bytes to clean up the stack. */ - TouchButton5cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("5cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 5, &doDistance); - TouchButton10cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("10cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 10, &doDistance); - - TouchButton20cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("20cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 20, &doDistance); - TouchButton40cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, F("40cm"), - TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 40, &doDistance); - TouchButtonDebug.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_RED, F("dbg"), + TouchButton5cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("5cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 50, &doDistance); + TouchButton10cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_2, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("10cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 100, &doDistance); + + TouchButton20cm.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("20cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 200, &doDistance); + TouchButton40cm.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("40cm"), + TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 400, &doDistance); + TouchButtonDebug.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_3, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_RED, F("dbg"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH | FLAG_BUTTON_TYPE_TOGGLE_RED_GREEN, false, &doShowDebug); - TouchButton45DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, +// TouchButtonReset.init(&ButtonReset, F("Reset")); +// TouchButton5cm.init(&Button5cm, F("5cm")); +// TouchButton10cm.init(&Button10cm, F("10cm")); +// +// TouchButton20cm.init(&Button20cm, F("20cm")); +// TouchButton40cm.init(&Button40cm, F("40cm")); +// TouchButtonDebug.init(&ButtonDebug, F("dbg")); + + TouchButton45DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("45\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 45, &doRotation); // \xB0 is degree character - TouchButton45DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton45DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("-45\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -45, &doRotation); // \xB0 is degree character - TouchButton360Degree.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_4, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton360Degree.init(BUTTON_WIDTH_8_POS_6, BUTTON_HEIGHT_8_LINE_5, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("360\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 360, &doRotation); // \xB0 is degree character - TouchButton90DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton90DegreeLeft.init(BUTTON_WIDTH_8_POS_4, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("90\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, 90, &doRotation); // \xB0 is degree character - TouchButton90DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR_BLUE, + TouchButton90DegreeRight.init(BUTTON_WIDTH_8_POS_5, BUTTON_HEIGHT_8_LINE_6, BUTTON_WIDTH_8, BUTTON_HEIGHT_8, COLOR16_BLUE, F("-90\xB0"), TEXT_SIZE_11, FLAG_BUTTON_DO_BEEP_ON_TOUCH, -90, &doRotation); // \xB0 is degree character + +// TouchButton45DegreeLeft.init(&Button45DegreeLeft, F("45\xB0")); // \xB0 is degree character +// TouchButton45DegreeRight.init(&Button45DegreeRight, F("-45\xB0")); // \xB0 is degree character +// TouchButton360Degree.init(&Button360Degree, F("360\xB0")); // \xB0 is degree character +// +// TouchButton90DegreeLeft.init(&Button90DegreeLeft, F("90\xB0")); // \xB0 is degree character +// TouchButton90DegreeRight.init(&Button90DegreeRight, F("-90\xB0")); // \xB0 is degree character } void drawTestPage(void) { @@ -139,7 +204,7 @@ void drawTestPage(void) { TouchButton5cm.drawButton(); TouchButton10cm.drawButton(); -#ifdef USE_ENCODER_MOTOR_CONTROL +#if defined(USE_ENCODER_MOTOR_CONTROL) || defined(USE_MPU6050_IMU) TouchButtonCalibrate.drawButton(); #endif @@ -153,7 +218,9 @@ void drawTestPage(void) { TouchButtonCompensationLeft.drawButton(); TouchButtonCompensationRight.drawButton(); +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonCompensationStore.drawButton(); +#endif TouchButton90DegreeLeft.drawButton(); TouchButton90DegreeRight.drawButton(); @@ -164,7 +231,9 @@ void drawTestPage(void) { SliderSpeedRight.drawSlider(); SliderSpeedLeft.drawSlider(); #endif +#ifdef SUPPORT_EEPROM_STORAGE TouchButtonGetAndStoreSpeed.drawButton(); +#endif SliderUSPosition.setValueAndDrawBar(sLastServoAngleInDegrees); SliderUSPosition.drawSlider(); @@ -174,7 +243,7 @@ void drawTestPage(void) { SliderIRDistance.drawSlider(); # endif - PWMDcMotor::MotorValuesHaveChanged = true; // trigger drawing of values + PWMDcMotor::MotorControlValuesHaveChanged = true; // trigger drawing of values } /* diff --git a/src/digitalWriteFast.h b/src/digitalWriteFast.h index 3bbfc97..6267ec1 100644 --- a/src/digitalWriteFast.h +++ b/src/digitalWriteFast.h @@ -1,12 +1,14 @@ /* - Optimized digital functions for AVR microcontrollers - by Watterott electronic (www.watterott.com) - based on http://code.google.com/p/digitalwritefast + Optimized digital functions for AVR microcontrollers + by Watterott electronic (www.watterott.com) + based on http://code.google.com/p/digitalwritefast */ #ifndef __digitalWriteFast_h_ #define __digitalWriteFast_h_ 1 +//#define SANGUINO_PINOUT //define for Sanguino pinout + // general macros/defines #ifndef BIT_READ # define BIT_READ(value, bit) ((value) & (1UL << (bit))) @@ -22,7 +24,7 @@ #endif #ifndef SWAP -#define SWAP(x,y) do{ (x)=(x)^(y); (y)=(x)^(y); (x)=(x)^(y); }while(0) +# define SWAP(x,y) do{ (x)=(x)^(y); (y)=(x)^(y); (x)=(x)^(y); }while(0) #endif #ifndef DEC @@ -38,8 +40,9 @@ # define BIN (2) #endif + // workarounds for ARM microcontrollers -#if (!defined(__AVR__) && !defined(ESP8266) || \ +#if (!defined(__AVR__) || \ defined(ARDUINO_ARCH_SAM) || \ defined(ARDUINO_ARCH_SAMD)) @@ -96,6 +99,7 @@ #endif + // digital functions //#ifndef digitalPinToPortReg #define SPI_SW_SS_PIN (10) //SS on Uno (for software SPI) @@ -103,6 +107,7 @@ #define SPI_SW_MISO_PIN (12) //MISO on Uno (for software SPI) #define SPI_SW_SCK_PIN (13) //SCK on Uno (for software SPI) + // --- Arduino Due and SAM3X8E based boards --- #if (defined(ARDUINO_SAM_DUE) || \ defined(__SAM3X8E__)) @@ -210,21 +215,21 @@ // --- Arduino MightyCore standard pinout --- -#elif defined(__AVR_ATmega1284P__) || \ - defined(__AVR_ATmega1284P__) || \ - defined(__AVR_ATmega644P__) || \ - defined(__AVR_ATmega644__) || \ - defined(__AVR_ATmega324PB__) || \ - defined(__AVR_ATmega324PA__) || \ - defined(__AVR_ATmega324P__) || \ - defined(__AVR_ATmega324A__) || \ - defined(__AVR_ATmega164P__) || \ - defined(__AVR_ATmega164A__) || \ - defined(__AVR_ATmega32__) || \ - defined(__AVR_ATmega16__) || \ - defined(__AVR_ATmega8535__) && \ - !defined(BOBUINO_PINOUT) && \ - !defined(SANGUINO_PINOUT) +#elif (defined(__AVR_ATmega1284P__) || \ + defined(__AVR_ATmega1284__) || \ + defined(__AVR_ATmega644P__) || \ + defined(__AVR_ATmega644A__) || \ + defined(__AVR_ATmega644__) || \ + defined(__AVR_ATmega324PB__) || \ + defined(__AVR_ATmega324PA__) || \ + defined(__AVR_ATmega324P__) || \ + defined(__AVR_ATmega324A__) || \ + defined(__AVR_ATmega164P__) || \ + defined(__AVR_ATmega164A__) || \ + defined(__AVR_ATmega32__) || \ + defined(__AVR_ATmega16__) || \ + defined(__AVR_ATmega8535__)) && \ + !defined(BOBUINO_PINOUT) #define UART_RX_PIN (8) //PD0 #define UART_TX_PIN (9) //PD1 @@ -241,22 +246,33 @@ #define __digitalPinToPortReg(P) \ (((P) >= 0 && (P) <= 7) ? &PORTB : (((P) >= 8 && (P) <= 15) ? &PORTD : (((P) >= 16 && (P) <= 23) ? &PORTC : (((P) >= 24 && (P) <= 31) ? &PORTA : &PORTE)))) #define __digitalPinToDDRReg(P) \ -(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 8 && (P) <= 15) ? &DDRC : (((P) >= 24 && (P) <= 31) ? &DDRA : &DDRE)))) +(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 16 && (P) <= 23) ? &DDRC : (((P) >= 24 && (P) <= 31) ? &DDRA : &DDRE)))) #define __digitalPinToPINReg(P) \ -(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 8 && (P) <= 15) ? &PINC : (((P) >= 24 && (P) <= 31) ? &PINA : &PINE)))) +(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 16 && (P) <= 23) ? &PINC : (((P) >= 24 && (P) <= 31) ? &PINA : &PINE)))) +# if defined(SANGUINO_PINOUT) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (((P) >= 16 && (P) <= 23) ? (7 - ((P) - 24)) : (P) - 32)))) +# else //MightyCore Pinout #define __digitalPinToBit(P) \ (((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (((P) >= 16 && (P) <= 23) ? (P) - 24 : (P) - 32)))) +# endif #else #define __digitalPinToPortReg(P) \ (((P) >= 0 && (P) <= 7) ? &PORTB : (((P) >= 8 && (P) <= 15) ? &PORTD : (((P) >= 16 && (P) <= 23) ? &PORTC : &PORTA))) #define __digitalPinToDDRReg(P) \ -(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 8 && (P) <= 15) ? &DDRC : &DDRA))) +(((P) >= 0 && (P) <= 7) ? &DDRB : (((P) >= 8 && (P) <= 15) ? &DDRD : (((P) >= 16 && (P) <= 23) ? &DDRC : &DDRA))) #define __digitalPinToPINReg(P) \ -(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 8 && (P) <= 15) ? &PINC : &PINA))) +(((P) >= 0 && (P) <= 7) ? &PINB : (((P) >= 8 && (P) <= 15) ? &PIND : (((P) >= 16 && (P) <= 23) ? &PINC : &PINA))) +# if defined(SANGUINO_PINOUT) +#define __digitalPinToBit(P) \ +(((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (7 - ((P) - 24))))) +# else //MightyCore Pinout #define __digitalPinToBit(P) \ (((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 15) ? (P) - 8 : (((P) >= 16 && (P) <= 23) ? (P) - 16 : (P) - 24))) +# endif #endif + // --- Arduino Leonardo and ATmega16U4/32U4 based boards --- #elif (defined(ARDUINO_AVR_LEONARDO) || \ defined(__AVR_ATmega16U4__) || \ @@ -330,7 +346,9 @@ (((P) >= 0 && (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) #endif -#elif defined(__AVR_ATmega4809__) // Uno WiFi Rev 2, Nano Every + +// --- Arduino Uno WiFi Rev 2, Nano Every --- +#elif defined(__AVR_ATmega4809__) #define UART_RX_PIN (0) //PB0 #define UART_TX_PIN (1) //PB1 @@ -353,48 +371,56 @@ (((P) == 2 || (P) == 9 || (P) == 11 || (P) == 17) ? 0 : ((P) == 7 || (P) == 10 || (P) == 12 || (P) == 16) ? 1 : ((P) == 5 || (P) == 13 || (P) == 15 || (P) == 18) ? 2 : ((P) == 9 || (P) == 14 || (P) == 19) ? 3 : ((P) == 6 || (P) == 20) ? 4 : ((P) == 3 || (P) == 21) ? 5 : 6 ) +// TinyCore +// https://raw.githubusercontent.com/xukangmin/TinyCore/master/avr/package/package_tinycore_index.json +// https://docs.tinycore.dev/en/latest/ +#elif defined(__AVR_ATtiny1616__) || defined(__AVR_ATtiny3216__) || defined(__AVR_ATtiny3217__) +#define __digitalPinToPortReg(P) ((P) <= 5 ? &VPORTB.OUT : ((P) <= 9 ? &VPORTC.OUT : ((P) <= 16 ? &VPORTA.OUT : ((P) <= 18 ? &VPORTB.OUT : &VPORTC.OUT)))) +#define __digitalPinToDDRReg(P) ((P) <= 5 ? &VPORTB.DIR : ((P) <= 9 ? &VPORTC.DIR : ((P) <= 16 ? &VPORTA.DIR : ((P) <= 18 ? &VPORTB.DIR : &VPORTC.DIR)))) +#define __digitalPinToPINReg(P) ((P) <= 5 ? &VPORTB.IN : ((P) <= 9 ? &VPORTC.IN : ((P) <= 16 ? &VPORTA.IN : ((P) <= 18 ? &VPORTB.IN : &VPORTC.IN)))) +#define __digitalPinToBit(P) ( (P) <= 3 ? (3 - P) : ((P) <= 5 ? (P) : ((P) <= 9 ? (P - 6) : ((P) <= 16 ? ((P) - 9) : ((P) <= 18 ? ((P) - 11) : ((P) - 15))))) ) + + // --- ATtinyX5 --- #elif defined(__AVR_ATtiny25__) || defined(__AVR_ATtiny45__) || defined(__AVR_ATtiny85__) // we have only PORTB #define __digitalPinToPortReg(P) (&PORTB) #define __digitalPinToDDRReg(P) (&DDRB) #define __digitalPinToPINReg(P) (&PINB) -#define __digitalPinToBit(P) (( (P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) +#define __digitalPinToBit(P) (((P) <= 7) ? (P) : (((P) >= 8 && (P) <= 13) ? (P) - 8 : (P) - 14)) // --- ATtiny88 --- #elif defined(__AVR_ATtiny88__) -# if defined(ARDUINO_AVR_DIGISPARKPRO) +# if defined(ARDUINO_AVR_DIGISPARKPRO) #define __digitalPinToPortReg(P) ((P) <= 7 ? &PORTD : ((P) <= 14 ? &PORTB : ((P) <= 18 ? &PORTA : &PORTC))) #define __digitalPinToDDRReg(P) ((P) <= 7 ? &DDRD : ((P) <= 14 ? &DDRB : ((P) <= 18 ? &DDRA : &DDRC))) #define __digitalPinToPINReg(P) ((P) <= 7 ? &PIND : ((P) <= 14 ? &PINB : ((P) <= 18 ? &PINA : &PINC))) #define __digitalPinToBit(P) ( (P) <= 7 ? (P) : ((P) <= 13 ? ((P) - 8) : ((P) == 14 ? 7 : ((P) <= 16 ? ((P) - 14) : ((P) <= 18 ? ((P) - 17) : ((P) == 25 ? 7 : ((P) - 19)))))) ) -# else +# else #define __digitalPinToPortReg(P) ((P) <= 7 ? &PORTD : ((P) <= 15 ? &PORTB : ((P) <= 22 ? &PORTC : ((P) <= 26 ? &PORTA : &PORTC)))) #define __digitalPinToDDRReg(P) ((P) <= 7 ? &DDRD : ((P) <= 15 ? &DDRB : ((P) <= 22 ? &DDRC : ((P) <= 26 ? &DDRA : &DDRC)))) #define __digitalPinToPINReg(P) ((P) <= 7 ? &PIND : ((P) <= 15 ? &PINB : ((P) <= 22 ? &PINC : ((P) <= 26 ? &PINA : &PINC)))) #define __digitalPinToBit(P) ((P) <= 15 ? ((P) & 0x7) : ((P) == 16 ? (7) : ((P) <= 22 ? ((P) - 17) : ((P) == 27 ? (6) : ((P) - 23))))) -# endif +# endif // --- ATtinyX4 + ATtinyX7 --- #elif defined(__AVR_ATtiny24__) || defined(__AVR_ATtiny44__) || defined(__AVR_ATtiny84__) || defined(__AVR_ATtiny87__) || defined(__AVR_ATtiny167__) -# if defined(ARDUINO_AVR_DIGISPARKPRO) -/// Strange enumeration of pins on Digispark board and core library +# if defined(ARDUINO_AVR_DIGISPARKPRO) +// Strange enumeration of pins on Digispark board and core library #define __digitalPinToPortReg(P) (((P) <= 4) ? &PORTB : &PORTA) #define __digitalPinToDDRReg(P) (((P) <= 4) ? &DDRB : &DDRA) #define __digitalPinToPINReg(P) (((P) <= 4) ? &PINB : &PINA) #define __digitalPinToBit(P) (((P) <= 2) ? (P) : (((P) == 3) ? 6 : (((P) == 4) ? 3 : (((P) == 5) ? 7 : (P) - 6 )))) - -# else -// ATtinyX4: PORTA for 0 to 7, PORTB for 8 to 11 -// ATtinyX7: PORTA for 0 to 7, PORTB for 8 to 15 +# else +// ATtinyX4: PORTA for 0 to 7, PORTB for 8 to 11 +// ATtinyX7: PORTA for 0 to 7, PORTB for 8 to 15 #define __digitalPinToPortReg(P) (((P) <= 7) ? &PORTA : &PORTB) #define __digitalPinToDDRReg(P) (((P) <= 7) ? &DDRA : &DDRB) #define __digitalPinToPINReg(P) (((P) <= 7) ? &PINA : &PINB) #define __digitalPinToBit(P) (((P) <= 7) ? (P) : (P) - 8 ) -# endif - +# endif // --- Other --- #else @@ -411,6 +437,7 @@ #endif //#endif //#ifndef digitalPinToPortReg + #ifndef digitalWriteFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define digitalWriteFast(P, V) \ @@ -424,6 +451,7 @@ if (__builtin_constant_p(P)) { \ #endif #endif + #ifndef pinModeFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define pinModeFast(P, V) \ @@ -442,6 +470,7 @@ if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ #endif #endif + #ifndef digitalReadFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define digitalReadFast(P) ( (int) __digitalReadFast((P)) ) @@ -454,6 +483,7 @@ if (__builtin_constant_p(P) && __builtin_constant_p(V)) { \ #endif #endif + #ifndef digitalToggleFast #if (defined(__AVR__) || defined(ARDUINO_ARCH_AVR)) #define digitalToggleFast(P) \ @@ -467,4 +497,5 @@ if (__builtin_constant_p(P)) { \ #endif #endif + #endif //__digitalWriteFast_h_