From 083b47cc33d2cafe84ad3cd58cd9a591340cfba7 Mon Sep 17 00:00:00 2001 From: Ben Hall Date: Thu, 21 Mar 2024 16:00:09 -0400 Subject: [PATCH] On car testing minimal (#45) * yo * Fixed all pindefs outside of setup * memes * yo * Added dash LED to safety system * walker eepy * working without adcs * some comments to debug * pushing current issues * working kinda minimal progress * working without steering sensor * pushing up constants and stuff * Reworked dashboard to use tick function and added LED states * pushing up debugging tcmux * torque requests * the negative sign was correct * fixing tcs and offsets * almost working kinda, error after a second sending no torque * working code * known good debug * yo * working on car * what is on car currently * working except for hardware issue * removed more print lines, simplified resetting of drivetrain, limited AMK max RPM for lot testing, re-enabled correct speed setpoint for torque control, and added in resetting of drivetrain via dash button * minor change in main_minimal for serial definition and bringing actual main up to where minimal main is now * Fixed pin defs for dashboard lights * Fixed IMD and AMS lights going red if BOTS tripped * Updated buzzer time and brake/accel thresholds * Fix for buzzer timing, force buzzer to end before entering ready to drive * Added new can message for inverter RPMs * Drivetrain RPMs are sent to TCU * Added DRIVETRAIN_ERR_STATUS_TELEM_hytech to telemetry * Added drivetrain_status_telem_CAN_msg() (untested) * Added some pedal data to drivetrain status message * Added accumulator current/reference can messages * Update to ADC interface to support setting 1M baud for MCP 04's. Changing pindefs for reading status of IMD and BMS OK * discovered potential issue with drivetrain system * fixed regen pedal scaling * Steering Sensor should now be working * add SAB reading to ECU * Added Motor Controller torque message and rolling average print for seal testing * Updated HT_CAN with new suspension messages. Removed old ones * removed fixed point precision on steering sensor. no clue why it was there * add load cell vectoring. add safe mode transition when controllers fault * working on car * attempting to remove deadzone in pedals for ensuring that we can always reach 100 percent and start at 0 percent accel and brake * put stuff for loadcell mode 1 calibrate * hopefully calibrated? * tick load cell controller * adding more to readme about auto-completition and setup of vscode * Added torque mode LED display * Changed torqueMode button * Updated Mock dashboard interface * adding in more docs to tc mux and tcs as well as adding in scaling for simple tc for regen * moving main minimal into main * only deploying docs on master --------- Co-authored-by: walkermburns Co-authored-by: Dopp-IO <42275088+Dopp-IO@users.noreply.github.com> Co-authored-by: Justin Hwang Co-authored-by: Dopp-IO Co-authored-by: shaynoorani <113149316+shaynoorani@users.noreply.github.com> --- .github/workflows/docs.yml | 6 +- .gitignore | 1 + README.md | 40 +- compile_commands.json | 1 - image-1.png | Bin 0 -> 19760 bytes include/MCU_rev12_dfs.h | 124 -- include/MCU_rev15_defs.h | 38 +- lib/interfaces/include/AMSInterface.h | 14 +- lib/interfaces/include/DashboardInterface.h | 27 +- lib/interfaces/include/HytechCANInterface.h | 11 + lib/interfaces/include/InverterInterface.h | 62 +- lib/interfaces/include/InverterInterface.tpp | 111 +- lib/interfaces/include/MCUInterface.h | 44 +- lib/interfaces/include/SABInterface.h | 51 + lib/interfaces/include/TelemetryInterface.h | 59 +- lib/interfaces/include/WatchdogInterface.h | 8 +- lib/interfaces/src/AMSInterface.cpp | 10 + lib/interfaces/src/DashboardInterface.cpp | 42 +- lib/interfaces/src/MCUInterface.cpp | 27 +- lib/interfaces/src/SABInterface.cpp | 17 + lib/interfaces/src/TelemetryInterface.cpp | 193 +- lib/interfaces/src/WatchdogInterface.cpp | 4 + lib/mock_interfaces/DashboardInterface.h | 15 + lib/mock_interfaces/WatchdogInterface.h | 2 +- lib/state_machine/include/MCUStateMachine.h | 4 + lib/state_machine/include/MCUStateMachine.tpp | 75 +- lib/systems/include/Buzzer.h | 12 +- lib/systems/include/DrivetrainSystem.h | 6 +- lib/systems/include/DrivetrainSystem.tpp | 101 +- lib/systems/include/PedalsSystem.h | 24 +- lib/systems/include/SafetySystem.h | 3 +- lib/systems/include/TorqueControllerMux.h | 30 +- lib/systems/include/TorqueControllers.h | 157 +- lib/systems/src/Buzzer.cpp | 8 +- lib/systems/src/PedalsSystem.cpp | 67 +- lib/systems/src/SafetySystem.cpp | 7 +- lib/systems/src/TorqueControllerMux.cpp | 52 +- lib/systems/src/TorqueControllers.cpp | 170 +- platformio.ini | 8 +- src/main.cpp | 207 ++- src/main_spi_test.cpp | 25 + src/old_main.cpp | 1576 ----------------- src/test_can_interface.cpp | 2 +- .../dashboard_interface_test.h | 2 +- test/test_systems/drivetrain_system_test.h | 42 +- test/test_systems/pedals_system_test.h | 8 +- test/test_systems/state_machine_test.h | 2 +- 47 files changed, 1329 insertions(+), 2166 deletions(-) delete mode 100644 compile_commands.json create mode 100644 image-1.png delete mode 100644 include/MCU_rev12_dfs.h create mode 100644 lib/interfaces/include/SABInterface.h create mode 100644 lib/interfaces/src/SABInterface.cpp create mode 100644 src/main_spi_test.cpp delete mode 100644 src/old_main.cpp diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index cdebaa348..fe7ed526b 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -1,6 +1,10 @@ name: generate doxygen docs and deploy to github pages -on: [push, pull_request] + +on: + push: + branches: + - master jobs: gen_docs: diff --git a/.gitignore b/.gitignore index 304e87297..f883b181d 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,5 @@ .vscode/launch.json .vscode/ipch .vscode/ +compile_commands.json docs/html/ diff --git a/README.md b/README.md index 4d9fea392..f4d5b0365 100644 --- a/README.md +++ b/README.md @@ -28,7 +28,7 @@ To test using platformio core CLI simply run `pio test -e test_env`. To upload to the teensy simply use the platformio upload arrow shown [here at number 3](https://docs.platformio.org/en/latest/integration/ide/vscode.html#platformio-toolbar). -#### On Implementing and Importance of tests +##### On Implementing and Importance of tests Unit tests are a great way to ensure that new features and new code in general can integrate and work well with other code. It provides a framework to verify that your stuff works the way it should and lets you know when and how it doesnt. This project uses unit tests in the `test` folder to for both local testing and testing in the CI on github. @@ -39,6 +39,44 @@ You can see results of previous test runs on commits here: https://github.com/hy These MUST be maintained for functionality of the car. +### setup vscode +##### with auto-completition and advanced navigation using compile commands + + +the `compile_commands.json` file is a file that gets generated in the build folder upon excution of the [compiledb platformio task](https://docs.platformio.org/en/latest/integration/compile_commands.html) that sets up the include paths for vscode. [more about compile_commands.json here](https://clangd.llvm.org/design/compile-commands). + +If you enter the platformio terminal, you can generate compile commands with +1. ![alt text](image-1.png) + +then +2. ```pio run -t compiledb -e teensy41``` + + +this will generate the compile commands for the `teensy41` environment which is the environment we use to build and flash to the car. in a similar fashion, other environments (eg: `test_env`) can have their compile commands generated. + +3. then you need to setup vscode to recognize where this file is, which gets put into the `.pio` folder in your workspace. To do this, simply create a `.vscode` folder in the workspace and add a file called `c_cpp_properties.json` and it should look like this: + +```json +{ + "configurations": [ + { + "name": "teensy41", + "includePath": [ + "${workspaceFolder}/**" + ], + "defines": [], + "cStandard": "c17", + "cppStandard": "gnu++17", + "intelliSenseMode": "linux-gcc-x64", + "compileCommands": "${workspaceFolder}/.pio/build/teensy41/compile_commands.json" + } + ], + "version": 4 +} +``` +then you can select this configuration to be used with [this c/c++ extension](https://marketplace.visualstudio.com/items?itemName=ms-vscode.cpptools) in vscode for auto-completition and code navigation. + +to select the configuration, use the hotkey `ctrl+shift+p` to open the command prompt in vscode, and then type \`C/C++: Select a Configuration\` and then select your configuration for which you named above. ## Design Lore of the Code diff --git a/compile_commands.json b/compile_commands.json deleted file mode 100644 index fe51488c7..000000000 --- a/compile_commands.json +++ /dev/null @@ -1 +0,0 @@ -[] diff --git a/image-1.png b/image-1.png new file mode 100644 index 0000000000000000000000000000000000000000..420acf17fbef99b69f939c9ce3165cfdfbbd8bac GIT binary patch literal 19760 zcmbTe1yq&o_bm#dh)9Wa8k9(bq<{i~bV`>XUD90wB2ogf1!<7(ZjhD+=`QIKkgl`# z@Bcsd+%fLGtY@vc=A3H>Dac7+W0GN_prBw&z7|tLK|w=>uRmj; z!=L*;;!*IwoAx4-Dj4v~1LNH%_&ce?D|H8D8)FCOw{}J-Ce}7qM$Gnxc1A|l_NF!t zyJ(F<@FHg9i$v{=-a44sSU*rPvobv!Tvz@t=$6-HV&=_>>T{hc=(@jKTwc; z@JdX@ueL-G1?2&Xq}WRpm-ww2XAgpr$)9_3h^>q$aeSrMK6n^-uSKE^UMhacNnN|8 zQlG>ZdSyi7!UpNM20zxtcYLy=SvL5Qkpw>BFL{( z!y;(@>zkkW#UoLWFT{AbK_YqMKVSG~8UDY%P-p5EUS(znwRb#ylE-iJi4vpk7(V-q z){&H|9H-K;W8?5#a~^k3+MN}*MvRBnESS|A>NAQ_Ja-R{kLe#g_-U@8sY&+a$rF6F zz{Q-&UfsDk!GfHbt;6W5xXsH){#Dhl@f8ApQThcS@8;&OnE!cS6#qZ>^}qc9{|7I0 zgw>mF2=RRzlQx+;TUAyOtdxf3nbmQ4nOrSJV3N z;ls(Q@U>(x5wijV0R^9(e_`X*_fnI7RmL4BK9&~_4qSe>aWmfJs@9!t zwSr2SktoXgq9M*le$5_nj>gp9|K#gF@ReY{ZfR7)ydJu5@98|8dtXtvr)jI8ZJY_E zw8oD3+2QfCG*eUhuM(^&8fD~|hcOp|Y=P&hj5seiIZWCXR^x0M9zK4IrlzJA#~Urxmij7~*X`dL_BVM9+s#Ot7b@hXdVZ5E zBB4XV{5O^h=GeO1?wZyQN{QN~5f;{YL3jcd5qmNej>dpUs|7qbupEEP*3meXI(ov^NP2?Xv ze!Mk43kK-9D;A!# zn|o)Vza=@tY9pAXC}Ux;<4}ly?r*#!frx2-BCS|>L3Eg5q&h{$f86|c9D{3D$d6+w z(x|#v)Vb$nsDdZw9YYzl+J4#2E7^O4Vb7x%$uMR2#-c?`@**!uYhHa&@?8m}e1pHr zAhD@9a+q&!HC17>+LwSpw;e0c3HtPDer+(VwXIEqtSzivXLH7RuC2Q}IO4IytX;<~ zChWttwVcy*bBI0N^$v

&<*Vs?Ct*XVQM2O?ymS|#-P&o zgDB~Gcixv#z0sM=AtEE|fSXl8rE4@yxd!NH=y=p_U zyXBYd%A_52b6$sfg!-xC_IcW{TUJHh&*2n-_D`c(VY|0~{@Dr0mV6q&;%z{f>%D07 zH8wlz>SScLK#b7V)tfM&a8{zJ^(qTT_viXIv@F~=c0PHPPd^jf?)%73c3k zq^T+^PEJo3mX`bq3t3Bzdmjl4Cmn519sHJNO&MArud;po{@I?qZ;Wu5iC?9Seg|)-1io(iP(v~!~ibmYtvcctV^3!?M zB{v!#5s~;@gXig=DET~u8rO~{DJf~?maRqbw{OI7It+Va*bzp7xYTqH9{Bj8-PVls z!6xTU9@~_ZmzT&Kx(AVC7%tfU^@%dcGjo-66FGd5X*a0Swok-R8& z(tL)wMLS+;vB&HmMW@(jk4ZkdQ@?bbXB}?z3g7?Dm&^O6Sqjp3Naql_ksMw7xPao0 zbkS#aoDhj}E5R=NvEJ#FvV;%vD%hF+_jzgKe+~$RG%q+So_MImn_OJh#Pl$zRFtkV ze8ptb8Pe}&{JCW%VE-9gV(BVFMvB>9|8IF$o&@cpxstUt3l$Yr2nqX#mO$KtDVsLs zlA79DWfc{bg{rCLs`55VZm3mMam#@?l&KbKo9t3?JT?^<317dq!pV=}G{LENJ6!B$ zZOl6=!F*_q6M6HKX4A!stXXb|cII{6I=tXDUV&r3gve6uFU{jQ4b;am_vg#H_bS0%b9-*w9t1(nOcVJ6Pywtc@2XMy_e%p}gK#9#F2nEsY|+ekHiLxUfW6luz1K_V)D9KylL3 zn|fjsfLB=(0p7y0xRRVZIzOLlRff^oLkw#hM~CmuYzqElbtoZeKx+AK65n znvGU)whoU>zChZqnvnh7f|n>=!|#T#$&C*B^PY;Gb`)k^^GMVD2ibgn>+ZOUi z`DVWqjN?_VV$F#%Q(NMu_Z4}$P^!~4-I!*}vvKA&ZK~}!Rc=A7;Gudt$wK2i>q=ki zyj$>ox_o0KH?XqunAvr@i(#`WY_i&(o|508Nb=_5;v%opmU`?Ix9Nb-+2N-B&U7`$ zl0RN0aW(#foY4J04`rN>C9a3b8LlJD z*3b+y7>q5=wk-WfPKwYl_*#yAEr^d*%9safd+EX+!<&&}YD z9AQ^%_K(lL@6bno!v)`r1g6*eWmvu`CSte6Lc&R1!HZE^Yiu0q^jdt_gF47 z{JX>~H1za=wL3LPK*I(LD?JU@2=9Ei*b~c1KuCBGA77+DQLwYC%b@KuA@@0<$aDh| z;q!E4(J5|wczQx!C>%5-+i@i^JU>tNLSrLSysRcW+~-3a(&5^K2wwSSzjE(MoK^S| z$g#|o!`zt6|5q(w@t=IXZ}!`plgEjrjD9<@m2BrXlP`;0c5I@)QGX0f>Yx~OX}l>bjX< zxkWYB+Z%dv;!@k;=-`ctii$Z6hPm zJv}l`+mmeC+(`WGB7@!B)J7^Ba^K0+*d2AU{m(n*goME5-pTmkpNohLvq#?i z?=G+7HU59Ukn;cJg*6p-A6N?`n-sp*sjIH78`F!c(($d5|JQGA=JqEy(9n91(ptCM z$Hu<>`}faWIIOss+H`yMo3Y1tw9T7bS~Aq ziu}L7YSY$^HW?>k=LJ5!?f?3HJM!l6li!rD|MP4t_1@yzoU0+MWM&w~DL(+p99(m|29EIvU$+$7%DhAYFmMycRlV^8GlxQ% zvg<$U9)W(FA?^_yn}Qtc=)H!9h98-kYUj2gp_eZ#gd4fYL$ab+#yWXLAM0)M9?V^{ z`$^io@O*>~NA824UuS3MLRa)tr{gDLI?D?SzR(N7w+ILbP>l5M!OCGgl%kh>L8+uH z=~Gl#w)1?91^4dVw4x#fe3XY$3-AM~xyxGHI)(+4{(uul$|~ zcUzLj)9p8|QMQ@GMs=5k-a!uwtK6pk|cFUMMJ3Fc~o>H454Re)>)e{@k18w!AoZ!9{CP*UEl|4v_XsFB)MP8-A{AZVQ^ zHDQmHK`e)o^Q=^_CK~Sl>C~=z-(;>M`<>sBQLDmoVKvFSLUT4gaL7JZB&NQFX?ySz6SA`sVdwXRzbBV8l ziP!;4J%9e(bgY2Vus!50Ja$i#@Z5qYDHb}F`xd*?)_7C3{c29}aC$dXNob-G9rfPJdyW=#quseiVch3`QBZ=3S<#U^f_8&P?59r{AF)V9mfAxB zJv@5&(6=p^Sc|M5xrWeybD912udtfhAJg+@H*Cjh4p!<`fDI3TRZ~y#?fF6(C zdYV`IqVejGH8m}bRLFzZt)9x~HkFGHm7p6NP&i8A$_R99GB$@bh40TS#J#+P8n3Tv zM-GH-W}nl^es68T6_ybbyUA(N_YQjGo&i)>0TY~!-k3aKyyK79T9Rml**9GqeLEWJh{Si_a& zZ+a{Fm9t1pS9iJ+=kd4cHWsx|e0)5`=g*%{&Nd4>p)X)z344&KPu*XMvx$t3Zadjqf+fQu z@!nz^g4!WnNfH{r{{%u`UxxSfRo&&$6osJM*VBX5DB#KbaD{#WS4P5TM+1$rQ#T$l zaYwlD)vwQlPyZerAs5#8?|hT0A(@nWtZ+ub#o3I^9# zmzJ}2u}E~PBZUldAUsZBE2z#mR*-5a=6(~korGKrKiH{(-9B(_+o)Ywq0nx1*GIenOW4x=9^JC z4-dt*Bc1{-S%h@c{N=&+N?Blb37CLc*n&PrR92R5XxsnIPmj$>@s0>$pL{!ouRTR9aHP7Atdder|vE z&#abRNJz*?m+(oq^PFb~oDi8p-Fo$EyY!KokJ#j?DvYuGjyb(f@PmK;{3(Lm3@hH- zmmpvU|)9uwz0-}*VE_>!)HyEIUmuhR~2I@f%C?UG|~I&RtVkWQj^{r`UCUydq+7PCP}h| zzn4G*qJj)r3*D;ObXD;iwElM-5%KXdc6N5mh&)R=R$>P<1NL@*11CojSN7YPZ-RJf z$AyP9dH1QXic?Hiw_msl-HpoR>>8>Z4bd3u!};?Gsv?KgBv<#v>Fn&R)6($9ej^od zp+P}GZy~M_M!j)7nMFlKzfIO0LAzg5DRzcCA4}_UTjSNPBwU+kJq`Dj_B($ zG?=H1mfn`)U{~Ia5ZTK?!HIjN zl~2Fx;fz}kJ;ysNDCNx;zTuOTaaRZJ_t22pcu|UxkGZ%wIubDn3Q_`<;e(2nw>b3s zHya7o^S?jQ^0ce*>vi=uhLeS_FE|17EJGKJOj5x895ml$Eqd39kA*HOs}g>=IW|XAcTw-=ydNdhv{WAt`q%PUN(v~ z|JNLF*K)Efh*;MJHEr-$ifAD6kzc-ixiwQu%&_yZ)OlAo(QV@ix6Q2O#N24U7AX`2 zIBYa@bgDHwfCB_B&kmQ$$8-%LMMAZ^Pe%4xBqhs%I4wtY_M91>%-h==KqTp<5Y-KT^1NU8c*_#3RuB+Sh;6FoZ>ZqojXe5}VAdS3XC%Kz+9v4iNXw9gYR2)# zBY_spFCY{^xD4I>+qhIT&z?m?c)%)7RN7FVo}T*q`zI?!4`~xL#<~lw4Y=1|! zY$75fn<2&Y0TvbY@OXZ8#UpQ6Tz5PLNCKI{lf2HJC@LzJt1H>o$1MxwF}k?93_IO3 z*B&cK;I&=!pP`vQDQ@|*yn1j@LP&G5XTtndZW;^8aaHzbYJcqnW?|nNXZ6n?F(}v7 z4(pLKoIzJtS7Ksf?*TU|WnwWwp6&6X%2K0Gb<+y~czbLuW$*oF-H_vP~^E|*?1FLmb^uqHQIJ`!%&x;t41blpzg zB?)WToiplw(rUK-HpJ=!>Ma)PyyA-(vhOD;-dWfg;$He}DffxGsQ#xHfH&gv5bJG) zM24Nd`{d-(fKg)$Bk&w`hB9RN98Nxo^4#4Vsl}E3&XtjvN$*yl7}CRZHCf^3H|F4O zIzt!cWVQ77_!#ZMp&c2pwr_V67k9rIGi$QXr+8UTHAEe4Pb#Ua{L1LP<6t?5Lrrai zLn+kUFHAVsZ2;+6k>PV8L3vnM`^-#eU7ZIGh0ME_vvP#t?9jH*PR)=TQ6{j#nLTcE zuE&B7wdVy&9XG|etmJBele&33ZN&UnU*E&Mfw$aO6|;;5(qpxzL;Pf{g>D2yM0S7Y zABu>GDlin0-Jf4y==1dp0+|oKdTDJ4L@b)kKReKi zysux@&iE!*Nh{3F;!Zl;)+~FET3EL)m96YAE+)nX#id$foLL5Vqo=fbU(+4088mcf zV^x6CbhtJRG#<1jh&?5#6;CD~CB>qP=O*N_lhtJ*s2d_)&gLpBz4o~w*iPh2fW7TM zAsJx2{kHom#5N^Hv~Hm9>i$SJ*FT*&g@4Tn40f1m-mS z6xmX6>EumR0$B~r@89q--lI|=*1P%CWeIiIJ<3*7`C{3OTcD99!)AknJ!(}^7GA;Zo=)&$34EwMo z2*3r^8w*@Me2`11wVSIy{u1xud)R0t=KBE|{eS7+eo4$%f^k~JQ*{AyFxhfjmdo5T(f#bqS7 z1L2=fG2R$h;5;K*S-blK(rx(|*NVH)dttzWmXhP2C`GE}|{_z9a+T?{(2}dZ4(JR8;&s-!-RwmCmH??oO~9kPSXc zbIiK79?!Y2%yw%;bdG*9^5z z*C{vO!8(?B8-nxUO;F5-dhn674hUhD#-VR7Huf9yW+xB$l)MNp*4-Np1g!@#!`PGj zt4G4I=V&KA3?V;OOZa1Xk;5o~R?bx@?nCydXVPJKIrHpXN1L#SaVhCx3c|wf zSI+gD{_q?;M@36*0kV%DOAZUIEY&M3hO2P4C!yu4naKjyv0v#aX*Du7-aGk; z%6&d;8n0e%%zS>;@TkOM)kzm%vVo?Anil@NsUXE0yJz$qB~R$0CibgJI`UtimT59; z;a5bu6uBsPG_TB=jimBDeE3kc!^zosX&~b9MfJ0kVK)kX2Y=%TS+l7Ezh%LREJ+%u zJ(iM1v)1!V@?WCt1LnB%07E@w0)}1{YCkkn!-aZDKt-tc{#{&Omwf-84cRX6P?U7U zE9*<5NM#+-rBUNqxLR7?zaeD(r+m&=`C$ojmNzJfKQs@lX_NW=Pe4c+~Bd=Uc3S@_nSj@5RV@o%0n0IS_VxFFYmvz7cG_Bdh>7Vo(Mm)_q>1l?Z_=NTsWYV> zH=eU-Drml0MffC1w-AJLT1dFI&d#NpPE6~(R)&TH{ARD8d-yMP;Mms#VjW1gm(Ub@ zR!M^zN*-XyMK|iGZ28OBj$j) zo8EEo4V&mh7`mswuuXS~9h^gZvq1>fsjTbgK~V`B?ov~uPHl77oG~x*Z(NR$n#m;g zF>smpxS_e!yfF5v?x;qjEh{q+GHeeYPrkePB1QEjB7x%%!JOyV+O}rO{+)+XnIqNxEQe^gwCUNZEEGzen_M!43udQNJ?0dyYh5R zXms!0r=MtSjNg44@ZrK^>TG3YWrFExk$qcbQPIgcV~NJyFv|S#dQ7UVk-($v#XJNh zZ_F=wAOolbG{bXN{*!Aw1a@-`Mwe7qu6e`zbao$QIC2dR=JVRGFbOzqFaApLQ?(5W z47WQ@&@rXB*V(z$VLHtlube3dW{HV(&W^ z%B&Nnjt_l13a-pxQ;a?4&oN5n$9gIfEC}y-3tmm3(m@pu+QqYFYchfWH4`JY%}~VZ z&(E2h$uFW~aTf>~Q}LRcYa-Hy)+xF7GK8K zUU-IVyhcf`vMm@i<#o~5i?yYTBF!H?I+sctN#;@Sf3&oy?Iy_h9R-DkQn;y2`dc1i zd0B&=8k{0Id~Va9zsmbAVejVOiJSOd6gytAy*}Q+jC;J>eIp?(s-sX&q}XUkoZrz> zzVz2?By)<2x;5=sUOdN?LntIP3mAGzJKL0;SbC~T9dTV!GR&fwx`XVijMzv3g5IQ; zi+_eCyiLRiPS*kX}2h ze0hG!J?tHytiRua#Fj>j=9qIk0gp2A#$vryOp_q(I?_=ZIuwm?D0@1~Tv25eRFft1 z+uakj6fFzR>mML75K*Af8yk_tuoY!VCkj8P&YbeRXs{K{wtYg(dXI|8xU|kTt;o(q z`Dv(lS~1yU6g)C~8CBI&=YIux_MM$vp{63GSqKV2?X!CN-@kujR9j<-3>hl|z@Fs# zm5`8NMtQ&Es(x(r@sHn!MPt3oZm^uGpdj=)MSlNGRaw)eX3$?i$qcsFZ;S*v2|#P^PC5y0Cu<9S7j@IzO`Aix^c|65_Dt(lBNk7Pjw9{#UILYj zX_3#mHMb8^XBbK_*__Nmo}>zWFRV}(i6`1^8sPVDu5@20TSVz9jg1jZV1ITOC6sKt z=l5+%oqn@TNe5B>)ZNLOtA!OmpLN)}ztwH)*7M!x%jbPP*geDGu}(D{7v?*9XXY_G z!+u^8Pw(OkL5S;@4{Ez@X506A#2=s4emB585y8Yc=KkN2-r7u5|03Y+Rb-s1h`` zC>!S^63{!g1Qs|+PL#XjVvCBdO`X4lA{rSH>svY+`_l(ySdNjXCkgjw!{zO&#`AhC zs(l)XaJq#*KQgSenO@aAigN=Fa`1B2{qMhjNH-H*DEZQ^UeX+oRdG;0BXawclvsY} z7~Mt3_KHlC2!}pTk%0hlbr?ofnbS=?!;b(`hR=P6!u9mj^qE|l`3R<8lRnQKL5C9t zD@}=^5+j0+Er;Inv0uBF`ttkx`_i%~@#Sr5&hZIWg`a|^kS0UTtcs+U(O)&6(|dX1 zLN8@%eCa!c$UVS*%6VYxYC&2PXvp@@GAKJS*a)R)sOGaC$Cy`rbSm*~vv&WjOy&;e zC44MJ-!@JZou2H0D+=Z$w{x=|J!|o?%4Z)HXP^~cDDx1`U`tv&zxVYyJ^j%8#460o z$Ls`ca{&`rUCL&>-o;4V@J_!6+1rW&^Nhb8u$U9YwTzPb!v})bF&aZYoguR3&f~! z9{PJ6vJ0AfvSb=oW9Lm&h(_rhQhnB)2KU{o^TI%8Tl&bbl0WqKt)|-XXL4=<(*m^1 zZ%&TaGIeL3hpVGERm`8+vRkd~{qzLxqSRzg$ivg)?@>h}Vt+V$NW1QlCQ@owac) zFp9tlH8We!)2v4*(_50Cc7#)|^i0_MKf_v9{~bC*cyYR__kfM<=l)4kBJN-P{@c9X z3m=xf?kyhC@!ZW7rGUOAzi*%f^-N-kO?>!v+MNK+fW=m$GS2%7v?kGwe&z;+Pv3Nw zSX@&R5q*@)jC%Ltlah;z%ly`&h*VeGVw^uJL%}S~)>&MD&UIA{HmsFbgOJC5=k>^% z?W-Af)4x$wH8pG&DW2&W5%_BVmDco6!faGMFe|GtJg6RrK5mVqiJkzqee6#+Lj)I{wdcHMISM z13u5QvNsqnTcGx@7L06nXu$K(3A<)JswA zUsa<(K>>@loU~zY&2N+19P^Zv^tt(hJjcb+2ZrEts9~d~LkkJFOp*X6$k-yuE!O9aeX6<>#fMTUJ!GVatL?e4eV z_(kaZ9eVpGj3xC!ky*M&WJgEn*NAUeuRnmJAw3HD-pTu9e~t3LwRZq|?w_gX~*+4oK9-4ot2L}<6 znn(f4G_M)&>?GsaS?4CRKi+Z_SS!d@Odq|ss;goL8_3R=RUM3n6!tBWJ|I2|y8Y8{ z`fw{oCL@PIxg35IbeIs>ODNXpMN@|Eb{m0x`^-JZvhFe4Y)oOm$UWEps=tNc(C%{BlJQ-iTy(7 zAs}NQjpbmOZw3>6KS+rdbxgF{+S++nERr!fQW~Kx^ocGXt}l1J-@W7Yddvp?VNmkk zFelZ587&+XRCQ)fvQVTT2GVbNSGaR+Rl zvc$whl;K>p@b*x0PSB=NfIb2rp2O}er4O+F2faLV!|Ebml9IwFEU(USs@4&W76+wT zaR&`g2{1A5-hINr@ZN=-o<7t3sR^jXyLAU)BcYddb#)}b9)YyY;8+Wd5xeXDAM+>o z2^z!oT$c$!|9&ka^9L;I^(mJi7KhSGBZ`c=i2^GXb4TjVcWPzgc_WY^r`gg7;|f?L zY(7tvGlskN0`O89v*x_ct&r~^Eo}&+GM3{-O$(twikcBIYqo-Mjn82X@ACZE1V%A% zZ05jR2vWB%NJ+PGDCWTv{y;#WmgA5?4J2V;J&|N=Pdr}~QYdaS`A5cO`V&GH^#nYF zt_uOg;0Y!`yN&Zhx8BXETcy#<6WZf8DPdljm1J)GD5rb1>Sn38IgdM14q{3bvp^58PCrjb6yta?@JP!#$Xs9Gdil1=Eo~q`d zSdur6#1c$1@o2d%N2?)wIk5W5eCN88HuwqL+F)_#hfs!ZWKZte=}erdjU8ktFuazR zUj=&!!U%-xM-aRqBuve;gZCevWJ-s}xQB$_apNX|p5kjs$pwi2i@z{XB{^f~j~?t=4JV1^hX#@E@P^4h1aP z4>+w9&s~Zl`+W>SlWqTW?=e!Gkdu=uGabBvi;D|B_LIizb75F2Tu?sN{$3gP#P~wR zk{3KA;N#;XWDHiyuFLk)V@y>{6Ze4Ai|BxLiRX9xOzm|7CU2ba(CSeTPJe-a1@0el z>QM;UGt30J)d6w_M#j$oaD`6h(NO-UNkg?zcUyt_ zP=d^+6O=BrppUM004@I0WrBUMB)89SG5HzcpS= z#Ll=^$?L4ki@ zU{q}E`uhpcd2?0sQ=NBbM-p#>0P-a&ss-FOmZQM<+#@I7e2T~f)_8U(;t`u&+{WAn4bb;elz1b74Q28;%S5CwH7vHylHEj03gS4#Rgg<85bzB1*6tFZ*dJXY6DLKMQ>Y9AfVpc+dqcL1A_=MccSA^nDUA0kcEmHR)5vp zzaiYg4w;|9$`E%rF);xt?juORvADt)U%?Mb%=N&+Q)r$nRwuy5vEY;I|2XzK;P4PHwrtE%{iEHfw__k5jv& zof!*fa!3}7OH0x-nN9l1Sy`Xpu!d0yc0u0j?CYb0R0t-V!IT1h^YR*1auS&PXa(QQ zOs%sPvpl52K?&;e(;3p|E^HukBfS18JiltOJ* z=j0r+t2ony=xJ?l=dViw*|5}hNgUX_DyUbRe@LQW@c_#}LhVTuj01zjeLxtH-YgBI zpq4|5SlnOkX40k$o3d#fEYxe%WEtsw3(*2DN?$-~FpZ@0)QG|C zPfb-79w}*1QWCX7{2Ddy`=2|FT|MtApqO}gc;ul!Jl>xA#@cZ50T4IXW3Py9Y-~^( zP8RP%-8t#^zG{i2m3H|PA@K>k3Sc?|FJlt23P2EOVgJ2?bMh%9r0h9NRoG|K-=uQ= zbC1ex?IHM0`rzkCxJ$`OZv$AcP52gwf&2>68wlO%A$jmo6w_{e3}F6=3Q-IH4lM+#Yt^0j#668PU( zly(Hwv@za+aOW^Ct?prc23bs5Ua$>Lq{WaG{nMwx04zs1P)fMqI6JY@!ziTc6tOv^ zc)*j;{(^yHeWH{e_L3Gh8Hs7Z`G@j%znc|lUIVP-qnW<~fWWES`%=S6BC)ostFLbb z5;2Rtc$x(*Jv|cqsch7aQz76tEs`BvXzTk!gL@nfiP&vdDft_LQ1vd|1w{9jPdko6Gyjr_hx zV?Ur~?*kg#18dekSkU%C1&?<9kn(6K4gn|U5jX<|1{9Kc`RZUugp|jcqU9s@W3ZV5 zfG6|;$Oj8bigYZ3)k3|-;R#fU3wTImH3G+~5BN5rwg*X#ID^&o;sSNgvCje;;e3$5RM5)yUV^QW?J zJb@nFXbrea%nP6f=^%z8Jng!gfb(rBJ2UadJkl}z(+4$1h7PQ7F;MftLbT9EtoLrQ zH3%}uJ`5`I*Ue1=1118e5mpR|L{&Etta^Q5#bde#tZv~b*vZX9d{gfm5q$cAi1rAT|B)nydHcc#(nXUX)rwH zxbgMUFK5K*JnDO|abW5CXGTUw1F)Pvzx+3Z5K6bH+e3#)UxkN9f*y2(2BfdhVl?k9 z($BgHT_n<00w}{P0G)C+3t)!8aQdrk6=W6KovrV#*K6I*l1~x>BbY{lQ4P4wVTqB0 z)6axc$qUO|h-X+2%VD69(@Lg9I`Ed&DCj&HQhEhDiUS!nxC>~H8& zG-^h5z~~F+5&I5GXBF2|sgtua#H5Y)LUVw}(S!*c<(rUwz&m`hT{%~^Sy0QwPK#|u zOiV1DV51@~E?%y82N@55K+D{tn!x912j;%OqapD0dR<>ScP|0_eZO0OJc;E~@Cn?4 z;7x6XP8fs0`)BC7RQ%uI&%TTn>tX;`BF_AxzkmN+SXd~lC#9!1rxdP32-&^^2EHYsRD)Rv>1hUk&J|$E zR0RfP&PO&xkX&G45&X->=H@xQOq9!l&TqCMYqW!!3s|VOR^a;TxG{GNOU)Iu18UEI z_n^KZn`;DmJb-JP#!KXcV#Ru``b>fBm#$z7=;ErERIMc-o{+5`6p~5p0=;^-XGxw% zq)@f<2Hg$8HWdke`$X_nY1Yg>A;1K8oNI;kj5%qeaugB-1KdA>LuLLP)n}1i0xqxu z$Smxk?F2|8Dg5mP>rz330*H4R%yy_nSBS37T+Z(uc9INo$1LV(T5) zQCnMEM8gT#fRSUv;9yjdY}*=Kcc?q)6F}5JGd*NSHw9o49AWpsTns=7oKs5K=g5{& zr^ca}-%n}QUTp+EB?0M4hqVPNlzxPRD22(SKXDmp(f+qKh#cQQb}V6(-M~xaqpQ!z z0Ki0kCIhnMgMQOJc?50j(#A#?w2$yn1^&Hm0U}MwFa4M}y+-^Sug;M3e}AFM0%uHQ zXKxQ?#I&rewd<+cu`hJzJUH`n*Ot$4ORdbzzpJ!77fX*7iTpa@H-4HdEyl*#%UoIJ zce_NKhVVnIMrk$!;`9-wy}AL3q7`eT)ay3WuV$=v?2e*JtIsd=TCEey%}P!BRy*fz z@9@+f&Tj8iUlz=HUKUu$xj6u`rvPFxD=*JqDcc_ivcU%@=f}IHLTiSl6KmVs=}M#O zOhf>M+1S`nZ~_gvF-{Vs1i=j_W@1AB3G6a@E%WL%v9*ukG%39IW=s9?8qd%!5rC=$2NP1J0m$ ze)#X7J_IXWxP7&D)ISAc;%}tiCgQU@J0-R2-3Xy{LJKCMr9}>D{{s}=*Xc?Q&d%5S zB!D~D*4EfMPaDBIU12%#LBM%O-J|}>D-;ksWctt}nEw&Y9MY)&@I@{&u0;MLt?xiT z89DhqN=g_HJRz0GyxjSqEnVREw#ijRBSsMJH!R@E@y5W{F;1p3ne@~1tHP?>z?!ew{{N{a{YCS5m^bAuK+^xDhN3a+JP)ms;2hi{)D|h+)k`L-#GY z$-(6f?5seD6lJM86O(2c2D~DEKRz!#GxIjo6jN*KcA#Sirlx{JLa>5|getA3K7z$s zi7*vw3hH^W-mHq7OyaSmN z7zX71oOlRrY;3IjAU~6VU&(oSSkO0qtgNgY6yi^?F6s48NT7r_3*jaUH6CXYpM-doYWW8&9YK6MK9ZL_NKcBXo==uJgEF?7aAt&eku&^*wD=X+@ z6A!ONIGuK;@1Xm=!CLI0g22XNe41V?ZS9ZFR)_Gr3!Sj3!4-fBrB&c;(J$q*9G@l?PD~k;x z%-(zx8cfgpoSC6CD2~Yqkk1^-{`?YM`Pn^AwwrGJ2{?KU9`McrH*fe3CP>H1eB9f! zh1o>EmIeKH?{3P<$|}oQ!{-XTU?6}%yis3Y4--@3eh#pP(Do%)Rgo}8V`2t0XBkYu zM-e>4!+>r!0B~elcJ>_r56GOS&BC)z{P*<0WPEB$KCWRuw|~B==_N*h$X}P|@JSpf zv$L}>2zByr{r1- zEz3?^Jv}{(9oCf?2r#2O?W!pta^UPrK-%o;?gn_|57QQ96%}N+c%!1CzPzS+EhQxa zXBG}Cd|ZO<(U!)KA3u`o>L_5xdV72AM~RIyAaw>J5JZ1ZJwR(z3XjvGe``u)d>Moq z(4hFFvdejNY)mVkChi$L=vM>+GG)t_%o1D*CeaN_|Hm3q)PU>ZJ7e%>_SM$ z${q#oUF#Kl0&L;{M`&)o*>fYu>`T?(IcM(t>jGwr7AM84Q_f8T_I(wEI+v|6`26;^ zIdHh{-m1#Y&m@X6KY#jEXF1tt@x=n*tP60L=#7$G1_n^q;={*}jV?+Xe{Ebo`=XwX z&Jkb(KkbT6*U1zo4H3WDZ3%k0$Af^CJxl-b!@&LraDU;4Cntrqj#~nkd!)DtFl=@B z=syiu%ubvrxVd+8&RgJE#)Q*P)s70LMTdrp0{3S;PxVrE6u6dJDDYuHPz{-EK+?+o-!0ucGa953#l+>EK zH!qtSfcuM7R8-Dn6uv!jgax>x#X9%bBYRFJj?Xc%v4Jr$J;1^u^8C^HeZbZvu%pas zH$VUL`%YjZbJ3bLZ{jUqItd)9KfQ;WiKA&o{W5N*G;U6&wDz4{Cfk6;--B1LxTboU zE`RPQ!1B@m%${{JK!)R)i97ii8sy8HJQy5+nq`4@H7S5l(mX{+29OGnB4ZT}uwauy mM`areBUn&?#qj(e`GC4jlMiih`UyPEjKR~@&t;ucLK6Vs!x8!b literal 0 HcmV?d00001 diff --git a/include/MCU_rev12_dfs.h b/include/MCU_rev12_dfs.h deleted file mode 100644 index 51622fb98..000000000 --- a/include/MCU_rev12_dfs.h +++ /dev/null @@ -1,124 +0,0 @@ -#pragma once - -/* - * Teensy Pin definitions - */ -#define INVERTER_EN 9 -#define INVERTER_24V_EN 8 - -#define MC_MAX_SPEED 20000 - -#define BRAKE_LIGHT_CTRL 7 - -#define ECU_CLK 13 -#define ECU_SDI 12 -#define ECU_SDO 11 - -#define STEERING_CS 10 -#define STEERING_SPI_SPEED 1000000 - -#define IMU_DATAREADY 35 -#define IMU_SYNC 37 -#define IMU_CS 38 -#define IMU_RESET 36 -#define VEHICLE_TILT_ANGLE_X 0.0261799 // (in radians) ~ 1.5 degrees -#define ACCL_DUE_TO_GRAVITY 9.80665 // from https://physics.nist.gov/cgi-bin/cuu/Value?gn - -#define CAN_ECU_RX1 22 -#define CAN_ECU_TX1 23 -#define CAN_ECU_RX2 0 -#define CAN_ECU_TX2 1 -#define CAN_ECU_RX3 30 -#define CAN_ECU_TX3 31 - -#define ADC1_CS 33 -#define ADC2_CS 29 -#define ADC3_CS 34 - -#define FR_INTERLOCK 40 -#define FL_INTERLOCK 39 -#define RR_INTERLOCK 16 -#define RL_INTERLOCK 14 - -#define INTERTIA_SENSE 20 -#define SOFTWARE_OK_READ 25 // SHDN_F_READ Watchdog Combined -#define BOTS_SENSE_OK_READ 26 // SHDN_G_READ -#define BOTS_OK_READ 3 // SHDN_B_READ -#define IMD_OK_READ 4 // SHDN_C_READ -#define BMS_OK_READ 5 // SHDN_D_READ -#define BSPD_OK_READ 6 // SHDN_E_READ -#define SOFTWARE_OK 28 -// digital outputs -#define WATCHDOG_INPUT 32 - -/* - * ADC pin definitions - */ -#define ADC_BRAKE_1_CHANNEL 2 -#define ADC_BRAKE_2_CHANNEL 3 -#define ADC_ACCEL_1_CHANNEL 0 -#define ADC_ACCEL_2_CHANNEL 1 - -#define ADC_STEERING_2_CHANNEL 4 - -#define ADC_GLV_READ_CHANNEL 4 - -#define ADC_FL_LOAD_CELL_CHANNEL 4 -#define ADC_FR_LOAD_CELL_CHANNEL 5 -#define ADC_RL_LOAD_CELL_CHANNEL 7 -#define ADC_RR_LOAD_CELL_CHANNEL 3 - -#define ADC_CURRENT_CHANNEL 5 -#define ADC_REFERENCE_CHANNEL 6 - -#define SUS_POT_FL 6 -#define SUS_POT_FR 7 -#define SUS_POT_RL 0 -#define SUS_POT_RR 2 -/* - * Shutdown read thresholds - */ - -#define SHUTDOWN_HIGH 350 //teensy ADC reading when the shutdown lines are at 5V - - -/* - * Other constants - */ -#define MIN_HV_VOLTAGE 60 // Volts in V * 1 - Used to check if Accumulator is energized - - -#define ALPHA 0.9772 // parameter for the sowftware filter used on ADC pedal channels -#define ADC_SPI_SPEED 1000000 -#define TORQUE_ADJUSTMENT_VOLTAGE 3.5242 - - -#define LOAD_CELL1_OFFSET -33.98 -#define LOAD_CELL1_SLOPE 0.1711 -#define LOAD_CELL2_OFFSET -35.59 -#define LOAD_CELL2_SLOPE 0.1762 -#define LOAD_CELL3_OFFSET -33.59 -#define LOAD_CELL3_SLOPE 0.192 -#define LOAD_CELL4_OFFSET -64.5 -#define LOAD_CELL4_SLOPE 0.1954 -// -// #define LOAD_CELL1_OFFSET 0 -// #define LOAD_CELL1_SLOPE 1 -// #define LOAD_CELL2_OFFSET 0 -// #define LOAD_CELL2_SLOPE 1 -// #define LOAD_CELL3_OFFSET 0 -// #define LOAD_CELL3_SLOPE 1 -// #define LOAD_CELL4_OFFSET 0 -// #define LOAD_CELL4_SLOPE 1 - -#define BMS_HEARTBEAT_TIMEOUT 30000 -#define DASH_HEARTBEAT_TIMEOUT 1000 -#define TIMER_SOFTWARE_ENABLE 100 - -#define PACK_CHARGE_CRIT_TOTAL_THRESHOLD 420 -#define PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD 35000 - -#define MECH_POWER_LIMIT 63 -#define DC_POWER_LIMIT 79 - -#define REGEN_OFF_START_THRESHOLD 1540 diff --git a/include/MCU_rev15_defs.h b/include/MCU_rev15_defs.h index b722aac21..a5fbf79e0 100644 --- a/include/MCU_rev15_defs.h +++ b/include/MCU_rev15_defs.h @@ -5,11 +5,9 @@ const int ADC1_CS = 34; const int ADC2_CS = 33; const int ADC3_CS = 29; -const int SOFTWARE_OK = 28; +const int SOFTWARE_OK = 8; const int WATCHDOG_INPUT = 32; -// serial def -HardwareSerial* STEERING_SERIAL = &Serial5; // ADC1 channel defs const int MCU15_ACCEL1_CHANNEL = 2; @@ -36,17 +34,31 @@ const unsigned long INV_CAN_BAUDRATE = 500000; const unsigned long TELEM_CAN_BAUDRATE = 500000; // APPS/Brake pedal parameters -const int ACCEL1_MIN_THRESH = 100; -const int ACCEL2_MIN_THRESH = 100; -const int ACCEL1_MAX_THRESH = 3000; -const int ACCEL2_MAX_THRESH = 3000; +const int ACCEL1_MIN_THRESH = 2087; +const int ACCEL2_MIN_THRESH = 1472; +const int ACCEL1_MAX_THRESH = 3283; +const int ACCEL2_MAX_THRESH = 323; const float APPS_ACTIVATION_PERCENTAGE = 0.1; -const int BRAKE1_MIN_THRESH = 100; -const int BRAKE2_MIN_THRESH = 100; -const int BRAKE1_MAX_THRESH = 3000; -const int BRAKE2_MAX_THRESH = 3000; -const float BRAKE_ACTIVATION_PERCENTAGE = 0.05; -const float BRAKE_MECH_THRESH = 0.05; // TODO: Determine actual mech threshold +const int BRAKE1_MIN_THRESH = 2757; +const int BRAKE2_MIN_THRESH = 867; +const int BRAKE1_MAX_THRESH = 1421; +const int BRAKE2_MAX_THRESH = 2198; +const float BRKAE_ACTIVATION_PERCENTAGE = 0.05; +const float BRAKE_MECH_THRESH = 0.55; +// Load Cell Defs to convert raw to lbs +// lbs = (scale)*raw + offset + +const float LOADCELL_FL_SCALE = 0.0553; +const float LOADCELL_FL_OFFSET = 15.892; + +const float LOADCELL_FR_SCALE = 0.0512; +const float LOADCELL_FR_OFFSET = 17.196; + +const float LOADCELL_RL_SCALE = 0.1147; +const float LOADCELL_RL_OFFSET = 21.842; + +const float LOADCELL_RR_SCALE = 0.0588; +const float LOADCELL_RR_OFFSET = 19.576; #endif /* __MCU15_H__ */ \ No newline at end of file diff --git a/lib/interfaces/include/AMSInterface.h b/lib/interfaces/include/AMSInterface.h index 16a936f9d..4227c3a37 100644 --- a/lib/interfaces/include/AMSInterface.h +++ b/lib/interfaces/include/AMSInterface.h @@ -4,10 +4,9 @@ #include "FlexCAN_T4.h" #include "HyTech_CAN.h" - -/// @brief Heartbeat Interval is the allowable amount of time between BMS status messages before car delatches */ -const unsigned long HEARTBEAT_INTERVAL = 20; // milliseconds -/// @brief The total pcc threshold is the lowest allowable voltage of the entire pack (in Volts)*/ +/* Heartbeat Interval is the allowable amount of time between BMS status messages before car delatches */ +const unsigned long HEARTBEAT_INTERVAL = 2000; // milliseconds +/* The total pcc threshold is the lowest allowable voltage of the entire pack (in Volts)*/ const unsigned long PACK_CHARGE_CRIT_TOTAL_THRESHOLD = 420; /* The lowest pcc threshold is the lowest allowable single cell voltage (in 100 microvolts)*/ const unsigned long PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD = 35000; //equivalent to 3.5V @@ -32,11 +31,8 @@ class AMSInterface filtered_max_cell_temp(init_temp), filtered_min_cell_voltage(init_volt), cell_temp_alpha(temp_alpha), - cell_voltage_alpha(volt_alpha) - { - // Set pin mode - pinMode(pin_software_ok_, OUTPUT); - } + cell_voltage_alpha(volt_alpha) {}; + /* Overloaded constructor that only takes in software OK pin and uses default voltages and temp*/ AMSInterface(int sw_ok_pin): AMSInterface(sw_ok_pin, DEFAULT_INIT_TEMP, DEFAULT_INIT_VOLTAGE, DEFAULT_TEMP_ALPHA, DEFAULT_VOLTAGE_ALPHA) {}; diff --git a/lib/interfaces/include/DashboardInterface.h b/lib/interfaces/include/DashboardInterface.h index afc7b1669..cbdc2983d 100644 --- a/lib/interfaces/include/DashboardInterface.h +++ b/lib/interfaces/include/DashboardInterface.h @@ -4,6 +4,21 @@ #include "MessageQueueDefine.h" #include "FlexCAN_T4.h" #include "hytech.h" +#include "MCUInterface.h" +#include "MCU_status.h" + +/* + Enum for the car's torque limits + MOVE ME! - ideally into a TorqueControllerDefs.h file + to prevent circular dependencies +*/ +enum class TorqueLimit_e +{ + TCMUX_LOW_TORQUE = 0, + TCMUX_MID_TORQUE = 1, + TCMUX_FULL_TORQUE = 2, + TCMUX_NUM_TORQUE_LIMITS = 3, +}; /* Enum for the modes on the dial, corresponds directly to dial index pos. */ enum class DialMode_e @@ -40,8 +55,8 @@ enum class DashLED_e INERTIA_LED, GLV_LED, CRIT_CHARGE_LED, - START_LED, - MC_ERROR_LED, + START_LED, /// from state machine. When READY_TO_DRIVE, set START_LED to true. See what else uses READY_TO_DRIVE so that you can update START_LED. + MC_ERROR_LED, /// from DrivetrainSystem.cpp, get drivetrain_error_occurred() IMD_LED, AMS_LED, }; @@ -112,9 +127,15 @@ class DashboardInterface @param can_msg is the reference to a new CAN message CAN_message_t */ void read(const CAN_message_t &can_msg); + /* write function will Pack a message based on the current data in the interface and push it to the tx buffer */ CAN_message_t write(); + /* + Tick DashboardInterface at 10hz to gather data and send CAN message + */ + void tick10(MCUInterface* mcu, int car_state, bool buzzer, bool drivetrain_error, TorqueLimit_e torque); + /*! getter for the dashboard's current dial position (drive profile) @return returns a DialMode_e enum with the current dial position @@ -129,7 +150,7 @@ class DashboardInterface /* getter for the mark button */ bool specialButtonPressed(); /* getter for the torque button (does not currently exist on dash ) */ - bool torqueButtonPressed(); + bool torqueModeButtonPressed(); /* getter for the inverter reset button (clears error codes ) */ bool inverterResetButtonPressed(); /* getter for the launch control button */ diff --git a/lib/interfaces/include/HytechCANInterface.h b/lib/interfaces/include/HytechCANInterface.h index 90b29c3d0..83b2b70b4 100644 --- a/lib/interfaces/include/HytechCANInterface.h +++ b/lib/interfaces/include/HytechCANInterface.h @@ -10,6 +10,7 @@ #include "InverterInterface.h" #include "DashboardInterface.h" #include "AMSInterface.h" +#include "SABInterface.h" /* struct holding interfaces processed by process_ring_buffer() @@ -27,6 +28,7 @@ struct CANInterfaces InverterInterface *rear_right_inv; DashboardInterface *dash_interface; AMSInterface *ams_interface; + SABInterface *sab_interface; }; // the goal with the can interface is that there exists a receive call that appends to a circular buffer @@ -70,7 +72,10 @@ template void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, unsigned long curr_millis) { // TODO switch to using the global CAN receive function from the generated CAN library + // if(rx_buffer.size() > 0){ + // Serial.println(rx_buffer.size()); + // } while (rx_buffer.available()) { CAN_message_t recvd_msg; @@ -136,6 +141,11 @@ void process_ring_buffer(BufferType &rx_buffer, const InterfaceType &interfaces, case ID_MC4_ENERGY: interfaces.rear_right_inv->receive_energy_msg(recvd_msg); break; + + // SAB msgs + case SAB_SUSPENSION_CANID: + interfaces.sab_interface->retrieve_pots_and_load_cells_CAN(recvd_msg); + break; } } } @@ -155,6 +165,7 @@ void send_all_CAN_msgs(bufferType &buffer, FlexCAN_T4_Base *can_interface) buffer.pop_front(buf, sizeof(CAN_message_t)); memmove(&msg, buf, sizeof(msg)); can_interface->write(msg); + // delayMicroseconds(2500); } } diff --git a/lib/interfaces/include/InverterInterface.h b/lib/interfaces/include/InverterInterface.h index e494e050c..fec95246b 100644 --- a/lib/interfaces/include/InverterInterface.h +++ b/lib/interfaces/include/InverterInterface.h @@ -12,7 +12,7 @@ #include "MCUInterface.h" #include - +#include /* Struct holding torque and speed setpoint */ struct InverterCommand { @@ -51,35 +51,67 @@ class InverterInterface void request_enable_inverter(); /* send MC command no torque message*/ void command_no_torque(); + void command_debug(); /* sends torque and rpm setpoints to MC */ void handle_command(const InverterCommand &command); /* send MC command reset message*/ void command_reset(); - /* check if MC is ready */ + /* Returns the value stored in system_ready_, which is + read from the MC in receive_status_msg() */ bool inverter_system_ready() { return system_ready_; } - /* check for errors */ - bool error() + /* Returns the value stored in error_, which is + read from the MC in receive_status_msg() */ + bool get_error() { return error_; } - /*check for dc quit on */ - bool dc_quit_on() + /* Returns the value stored in error_, which is + read from the MC in receive_status_msg() */ + bool get_warning() { + return warning_; + } + + + /* Returns the value stored in quit_dc_on_, which is + read from the MC in receive_status_msg() */ + bool get_quit_dc_on() { return quit_dc_on_; } - /* check for quit inverter on*/ - bool quit_inverter_on() + /* Returns the value stored in dc_on_, which is + read from the MC in receive_status_msg() */ + bool get_dc_on() + { + return dc_on_; + } + + /* Returns the value stored in quit_inverter_on_, which is + read from the MC in receive_status_msg() */ + bool get_quit_inverter_on() { return quit_inverter_on_; } + /* Returns the value stored in inverter_on_, which is + read from the MC in receive_status_msg() */ + bool get_inverter_on() + { + return inverter_on_; + } + + /* Returns the value stored in derating_on_, which is + read from the MC in receive_status_msg() */ + bool get_derating_on() { + return derating_on_; + } + /* check for dc bus voltage */ uint16_t dc_bus_voltage() { @@ -90,21 +122,31 @@ class InverterInterface float get_torque_current() {return torque_current_;} float get_mag_current() {return magnetizing_current_;} float get_actual_torque() {return actual_torque_nm_; } + uint16_t get_error_status(); private: float id110_val_; // for scaling to proper iq and id vals float torque_current_, magnetizing_current_; // iq and id in A respectively float actual_torque_nm_; /* write setpoints message to the CAN buffer */ - void write_cmd_msg_to_queue_(MC_setpoints_command &msg); + void write_cmd_msg_to_queue_(MC_setpoints_command msg); int16_t speed_; uint16_t dc_bus_voltage_; + + /* Booleans to store data from the MC. Updated in receive_status_msg() */ + bool system_ready_; bool error_; + bool warning_; bool quit_dc_on_; + bool dc_on_; bool quit_inverter_on_; - bool system_ready_; + bool inverter_on_; + bool derating_on_; + message_queue *msg_queue_; uint32_t can_id_; + Metro timer_can_ = Metro(20); + uint16_t diagnostic_number_; }; #include "InverterInterface.tpp" diff --git a/lib/interfaces/include/InverterInterface.tpp b/lib/interfaces/include/InverterInterface.tpp index c80180688..1712bd45b 100644 --- a/lib/interfaces/include/InverterInterface.tpp +++ b/lib/interfaces/include/InverterInterface.tpp @@ -1,22 +1,31 @@ #include "InverterInterface.h" template -void InverterInterface::write_cmd_msg_to_queue_(MC_setpoints_command &msg_in) -{ - CAN_message_t msg; - msg.id = can_id_; - msg.len = sizeof(msg_in); - msg_in.write(msg.buf); - uint8_t buf[sizeof(CAN_message_t)]; - memmove(buf, &msg, sizeof(CAN_message_t)); - msg_queue_->push_back(buf, sizeof(CAN_message_t)); +void InverterInterface::write_cmd_msg_to_queue_(MC_setpoints_command msg_in) +{ + auto test = msg_in; + if(timer_can_.check()){ + CAN_message_t msg; + msg.id = can_id_; + msg.len = sizeof(msg_in); + test.write(msg.buf); + uint8_t buf[sizeof(msg)]; + memmove(buf, &msg, sizeof(msg)); + msg_queue_->push_back(buf, sizeof(CAN_message_t)); + } } template void InverterInterface::request_enable_hv() { - MC_setpoints_command mc_setpoints_command{}; + MC_setpoints_command mc_setpoints_command; + mc_setpoints_command.set_speed_setpoint(0); + mc_setpoints_command.set_pos_torque_limit(0); + mc_setpoints_command.set_neg_torque_limit(0); + mc_setpoints_command.set_remove_error(false); + mc_setpoints_command.set_driver_enable(false); + mc_setpoints_command.set_inverter_enable(false); mc_setpoints_command.set_hv_enable(true); write_cmd_msg_to_queue_(mc_setpoints_command); } @@ -24,18 +33,21 @@ void InverterInterface::request_enable_hv() template void InverterInterface::request_enable_inverter() { - MC_setpoints_command mc_setpoints_command{}; + MC_setpoints_command mc_setpoints_command; mc_setpoints_command.set_speed_setpoint(0); mc_setpoints_command.set_pos_torque_limit(0); mc_setpoints_command.set_neg_torque_limit(0); + mc_setpoints_command.set_remove_error(false); mc_setpoints_command.set_driver_enable(true); + mc_setpoints_command.set_hv_enable(true); mc_setpoints_command.set_inverter_enable(true); + // Serial.println("requesting enabling inverter"); write_cmd_msg_to_queue_(mc_setpoints_command); } template void InverterInterface::disable() { - MC_setpoints_command mc_setpoints_command{}; + MC_setpoints_command mc_setpoints_command; mc_setpoints_command.set_inverter_enable(false); mc_setpoints_command.set_hv_enable(false); mc_setpoints_command.set_driver_enable(false); @@ -45,42 +57,54 @@ void InverterInterface::disable() mc_setpoints_command.set_neg_torque_limit(0); write_cmd_msg_to_queue_(mc_setpoints_command); } +template +void InverterInterface::command_debug() +{ + MC_setpoints_command mc_setpoints_command; + + mc_setpoints_command.set_speed_setpoint(120); + mc_setpoints_command.set_pos_torque_limit(1000); + mc_setpoints_command.set_neg_torque_limit(0); + mc_setpoints_command.set_driver_enable(true); + mc_setpoints_command.set_hv_enable(true); + mc_setpoints_command.set_inverter_enable(true); + write_cmd_msg_to_queue_(mc_setpoints_command); +} + template void InverterInterface::command_no_torque() { - MC_setpoints_command mc_setpoints_command{}; + MC_setpoints_command mc_setpoints_command; + mc_setpoints_command.set_speed_setpoint(0); mc_setpoints_command.set_pos_torque_limit(0); mc_setpoints_command.set_neg_torque_limit(0); - + mc_setpoints_command.set_driver_enable(true); + mc_setpoints_command.set_hv_enable(true); + mc_setpoints_command.set_inverter_enable(true); write_cmd_msg_to_queue_(mc_setpoints_command); } template void InverterInterface::handle_command(const InverterCommand &command) { - MC_setpoints_command mc_setpoints_command{}; - // TODO handle the correct conversion to the over the wire data from real-world data type - mc_setpoints_command.set_speed_setpoint(command.speed_setpoint_rpm); - - if (command.torque_setpoint_nm < 0) - { - mc_setpoints_command.set_neg_torque_limit(abs(command.torque_setpoint_nm)); - mc_setpoints_command.set_pos_torque_limit(0); - } - else - { - mc_setpoints_command.set_neg_torque_limit(0); - mc_setpoints_command.set_pos_torque_limit(command.torque_setpoint_nm); - } - + MC_setpoints_command mc_setpoints_command; + mc_setpoints_command.set_driver_enable(true); + mc_setpoints_command.set_hv_enable(true); + mc_setpoints_command.set_inverter_enable(true); + int16_t torque_cmd = (command.torque_setpoint_nm/21.42)*1000; + + // Serial.println(torque_cmd); + mc_setpoints_command.set_speed_setpoint((int16_t)command.speed_setpoint_rpm); + mc_setpoints_command.set_neg_torque_limit(-abs(torque_cmd)); + mc_setpoints_command.set_pos_torque_limit(abs(torque_cmd)); write_cmd_msg_to_queue_(mc_setpoints_command); } template void InverterInterface::command_reset() { - MC_setpoints_command mc_setpoints_command{}; + MC_setpoints_command mc_setpoints_command; mc_setpoints_command.set_remove_error(true); write_cmd_msg_to_queue_(mc_setpoints_command); } @@ -88,17 +112,26 @@ void InverterInterface::command_reset() template void InverterInterface::receive_status_msg(CAN_message_t &msg) { - MC_status mc_status(&msg.buf[0]); + MC_status mc_status; + mc_status.load(msg.buf); + system_ready_ = mc_status.get_system_ready(); + error_ = mc_status.get_error(); + warning_ = mc_status.get_warning(); quit_dc_on_ = mc_status.get_quit_dc_on(); + dc_on_ = mc_status.get_dc_on(); quit_inverter_on_ = mc_status.get_quit_inverter_on(); + inverter_on_ = mc_status.get_inverter_on(); + derating_on_ = mc_status.get_derating_on(); + speed_ = mc_status.get_speed(); // TODO FIXME see 8.2.3 Units on page 83 of // https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf#page=83&zoom=100,76,82 // for the actual conversion. requires looking at the current params of the inverter // to get scalar for this. - torque_current_ = ((float)mc_status.get_torque_current() * id110_val_) / 16384.0; // iq + // torque_current_ = ((float)mc_status.get_torque_current() * id110_val_) / 16384.0; // iq + torque_current_ = mc_status.get_torque_current(); magnetizing_current_ = ((float)mc_status.get_magnetizing_current() * id110_val_) / 16384.0; // id // TODO enable this on the inverters @@ -106,7 +139,12 @@ void InverterInterface::receive_status_msg(CAN_message_t &msg) // https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/pdk_205481_kw26-s5-fse-4q_en_.pdf#page=75 // it is given in units of 0.1% Mn or 0.1% of the max torque 9.8 Nm // actual_torque_nm_ = ((float)mc_status.get_actual_torque_value()) / (.001 * 9.8); - error_ = mc_status.get_error(); + + // if(error_) + // { + // Serial.println("got error in dt"); + // Serial.println(can_id_); + // } } // TODO fill this in with the correct receiving @@ -122,4 +160,11 @@ void InverterInterface::receive_temp_msg(CAN_message_t &msg) { MC_temps mc_temps(&msg.buf[0]); // TODO use this (not currently being used in old main) + diagnostic_number_ = mc_temps.get_diagnostic_number(); +} + +template +uint16_t InverterInterface::get_error_status() +{ + return diagnostic_number_; } \ No newline at end of file diff --git a/lib/interfaces/include/MCUInterface.h b/lib/interfaces/include/MCUInterface.h index 3b380c665..3ecb99019 100644 --- a/lib/interfaces/include/MCUInterface.h +++ b/lib/interfaces/include/MCUInterface.h @@ -7,14 +7,17 @@ #include "MessageQueueDefine.h" #include "PedalsSystem.h" -const int DEFAULT_BMS_OK_READ = 5; // SHDN_D_READ -const int DEFAULT_IMD_OK_READ = 4; // SHDN_C_READ -const int DEFAULT_BSPD_OK_READ = 6; // SHDN_E_READ -const int DEFAULT_SOFTWARE_OK_READ = 25; // SHDN_F_READ Watchdog Combined -const int DEFAULT_BOTS_OK_READ = 3; // SHDN_B_READ -const int DEFAULT_BRAKE_LIGHT_CTRL = 7; +const int DEFAULT_BMS_OK_READ = 17; // SHDN_D_READ +const int DEFAULT_BMS_SENSE_PIN = 16; // BMS_OK_SENSE +const int DEFAULT_IMD_OK_READ = 10; // SHDN_C_READ +const int DEFAULT_IMD_SENSE_PIN = 18; // OKHS_SENSE +const int DEFAULT_BSPD_OK_READ = 39; // SHDN_E_READ +const int DEFAULT_SOFTWARE_OK_READ = 25; // SHDN_F_READ Watchdog Combined +const int DEFAULT_BOTS_OK_READ = 24; // SHDN_B_READ +const int DEFAULT_BRB_OK_READ = 26; // SHDN_G_READ +const int DEFAULT_BRAKE_LIGHT_CTRL = 6; const int DEFAULT_INVERTER_ENABLE = 9; -const int DEFAULT_INVERTER_24V_ENABLE = 8; +const int DEFAULT_INVERTER_24V_ENABLE = 7; /// @brief specifically designed so that Walker would be happy struct MainECUHardwareReadPins @@ -25,6 +28,7 @@ struct MainECUHardwareReadPins int pin_bspd_ok_read; int pin_software_ok_read; int pin_bots_ok_read; + int pin_brb_ok_read; // brake light pin int pin_brake_light_ctrl; // inverter enable pins @@ -33,7 +37,7 @@ struct MainECUHardwareReadPins }; static const MainECUHardwareReadPins DEFAULT_PINS = {DEFAULT_BMS_OK_READ,DEFAULT_IMD_OK_READ, DEFAULT_BSPD_OK_READ, DEFAULT_SOFTWARE_OK_READ, - DEFAULT_BOTS_OK_READ, DEFAULT_BRAKE_LIGHT_CTRL, DEFAULT_INVERTER_ENABLE, DEFAULT_INVERTER_24V_ENABLE}; + DEFAULT_BOTS_OK_READ, DEFAULT_BRB_OK_READ, DEFAULT_BRAKE_LIGHT_CTRL, DEFAULT_INVERTER_ENABLE, DEFAULT_INVERTER_24V_ENABLE}; class MCUInterface { @@ -50,11 +54,13 @@ class MCUInterface bool imd_ok_high; bool bspd_ok_high; bool software_ok_high; - /* Shutdown circuit voltage */ - bool shutdown_b_above_threshold; - bool shutdown_c_above_threshold; - bool shutdown_d_above_threshold; - bool shutdown_e_above_threshold; + bool brb_ok_high; + /* Shutdown circuit voltage */ + bool shutdown_b_above_threshold; // BOTS + bool shutdown_c_above_threshold; // IMD + bool shutdown_d_above_threshold; // AMS + bool shutdown_e_above_threshold; // BSPD + bool shutdown_g_above_threshold; // BRB /* Private utility functions */ // Read all shutdown signals on ECU @@ -70,13 +76,8 @@ class MCUInterface MCUInterface(CANBufferType *msg_output_queue, const MainECUHardwareReadPins &pins): // Member initialization list msg_queue_(msg_output_queue), - pins_(pins) - { - // Set pin mode - pinMode(pins_.pin_inv_en, OUTPUT); - pinMode(pins_.pin_inv_24V_en, OUTPUT); - pinMode(pins_.pin_brake_light_ctrl, OUTPUT); - }; + pins_(pins){}; + // Overloading constructor MCUInterface(CANBufferType *msg_output_queue): MCUInterface(msg_output_queue, DEFAULT_PINS) @@ -96,6 +97,9 @@ class MCUInterface /* Feed to state machine */ bool bms_ok_is_high(); bool imd_ok_is_high(); + bool brb_ok_is_high(); + + bool get_bots_ok(); /* Update MCU_status CAN (main loop) */ // State machine diff --git a/lib/interfaces/include/SABInterface.h b/lib/interfaces/include/SABInterface.h new file mode 100644 index 000000000..32cc5023c --- /dev/null +++ b/lib/interfaces/include/SABInterface.h @@ -0,0 +1,51 @@ +#ifndef __SABINTERFACE_H__ +#define __SABINTERFACE_H__ + +#include "hytech.h" +#include "FlexCAN_T4.h" +#include "AnalogSensorsInterface.h" + +class SABInterface +{ +private: +public: + AnalogChannel rlLoadCell; + AnalogChannel rrLoadCell; + AnalogChannel pot3; + AnalogChannel pot4; + + SABInterface( + float rlLoadCellScale, + float rlLoadCellOffset, + float rrLoadCellScale, + float rrLoadCellOffset, + float pot3Scale, + float pot3Offset, + float pot4Scale, + float pot4Offset + ) + { + rlLoadCell.scale = rlLoadCellScale; + rlLoadCell.offset = rlLoadCellOffset; + + rrLoadCell.scale = rrLoadCellScale; + rrLoadCell.offset = rrLoadCellOffset; + + pot3.scale = pot3Scale; + pot3.offset = pot3Offset; + + pot4.scale = pot4Scale; + pot4.offset = pot4Offset; + }; + + SABInterface( + float rlLoadCellScale, + float rlLoadCellOffset, + float rrLoadCellScale, + float rrLoadCellOffset + ) : SABInterface (rlLoadCellScale, rlLoadCellOffset, rrLoadCellScale, rrLoadCellOffset, 1.0, 0.0, 1.0, 0.0) {}; + + void retrieve_pots_and_load_cells_CAN(CAN_message_t &recvd_msg); +}; + +#endif /* __SABINTERFACE_H__ */ diff --git a/lib/interfaces/include/TelemetryInterface.h b/lib/interfaces/include/TelemetryInterface.h index 07acb33f1..63499ef48 100644 --- a/lib/interfaces/include/TelemetryInterface.h +++ b/lib/interfaces/include/TelemetryInterface.h @@ -8,6 +8,10 @@ #include "MessageQueueDefine.h" #include "AnalogSensorsInterface.h" #include "SteeringEncoderInterface.h" +#include "hytech.h" +#include "InverterInterface.h" + +using InvInt_t = InverterInterface; const int FIXED_POINT_PRECISION = 10000; @@ -32,9 +36,6 @@ class TelemetryInterface private: /* Outbound telemetry CAN messages */ MCU_pedal_readings mcu_pedal_readings_; - MCU_load_cells mcu_load_cells_; - MCU_front_potentiometers mcu_front_potentiometers_; - MCU_rear_potentiometers mcu_rear_potentiometers_; MCU_analog_readings mcu_analog_readings_; /* CAN Tx buffer */ CANBufferType *msg_queue_; @@ -54,11 +55,9 @@ class TelemetryInterface const AnalogConversion_s &brake1, const AnalogConversion_s &brake2 ); - void update_load_cells_CAN_msg( + void update_suspension_CAN_msg( const AnalogConversion_s &lc_fl, - const AnalogConversion_s &lc_fr - ); - void update_potentiometers_CAN_msg( + const AnalogConversion_s &lc_fr, const AnalogConversion_s &pots_fl, const AnalogConversion_s &pots_fr ); @@ -69,6 +68,37 @@ class TelemetryInterface const AnalogConversion_s &reference, const AnalogConversion_s &glv ); + void update_drivetrain_rpms_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr + ); + void update_drivetrain_err_status_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr + ); + void update_drivetrain_status_telem_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr, + bool accel_implaus, + bool brake_implaus, + float accel_per, + float brake_per + ); + void update_drivetrain_torque_telem_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr); + void update_penthouse_accum_CAN_msg( + const AnalogConversion_s ¤t, + const AnalogConversion_s &reference + ); /* Enqueue outbound telemetry CAN messages */ // void enqueue_CAN_mcu_pedal_readings(); @@ -80,14 +110,25 @@ class TelemetryInterface template void enqueue_CAN(T can_msg, uint32_t id); + template + void enqueue_new_CAN(U* structure, uint32_t (* pack_function)(U*, uint8_t*, uint8_t*, uint8_t*)); + /* Tick at 50Hz to send CAN */ void tick( const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, - const SteeringEncoderConversion_s &encoder + const SteeringEncoderConversion_s &encoder, + const InvInt_t* fl, + const InvInt_t* fr, + const InvInt_t* rl, + const InvInt_t* rr, + bool accel_implaus, + bool brake_implaus, + float accel_per, + float brake_per ); }; -#endif /* TELEMETRYINTERFACE */ +#endif /* TELEMETRYINTERFACE */ \ No newline at end of file diff --git a/lib/interfaces/include/WatchdogInterface.h b/lib/interfaces/include/WatchdogInterface.h index 17e005a6c..24bd5ae03 100644 --- a/lib/interfaces/include/WatchdogInterface.h +++ b/lib/interfaces/include/WatchdogInterface.h @@ -3,7 +3,7 @@ #include -const unsigned long WATCHDOG_KICK_INTERVAL = 7; // milliseconds +const unsigned long WATCHDOG_KICK_INTERVAL = 10; // milliseconds class WatchdogInterface { @@ -18,11 +18,7 @@ class WatchdogInterface int pin_watchdog_input_; public: - WatchdogInterface(int wd_input_pin): pin_watchdog_input_(wd_input_pin) - { - // Set pin mode - pinMode(pin_watchdog_input_, OUTPUT); - } + WatchdogInterface(int wd_input_pin): pin_watchdog_input_(wd_input_pin){}; /* Initialize interface pin mode */ void init(unsigned long curr_millis); diff --git a/lib/interfaces/src/AMSInterface.cpp b/lib/interfaces/src/AMSInterface.cpp index efeaf0b31..14af00e5a 100644 --- a/lib/interfaces/src/AMSInterface.cpp +++ b/lib/interfaces/src/AMSInterface.cpp @@ -1,6 +1,9 @@ #include "AMSInterface.h" void AMSInterface::init(unsigned long curr_millis) { + + // Set pin mode + pinMode(pin_software_ok_, OUTPUT); set_heartbeat(curr_millis); @@ -25,6 +28,13 @@ void AMSInterface::set_heartbeat(unsigned long curr_millis) { } bool AMSInterface::heartbeat_received(unsigned long curr_millis) { + // if((curr_millis - last_heartbeat_time) >= HEARTBEAT_INTERVAL){ + // Serial.println("ERROR"); + // Serial.println(curr_millis); + // Serial.println(last_heartbeat_time); + // Serial.println(curr_millis - last_heartbeat_time); + // } + return ((curr_millis - last_heartbeat_time) < HEARTBEAT_INTERVAL); } diff --git a/lib/interfaces/src/DashboardInterface.cpp b/lib/interfaces/src/DashboardInterface.cpp index 30da940b4..e4ab8f14e 100644 --- a/lib/interfaces/src/DashboardInterface.cpp +++ b/lib/interfaces/src/DashboardInterface.cpp @@ -32,7 +32,6 @@ CAN_message_t DashboardInterface::write() dash_mcu_state.drive_buzzer = _data.buzzer_cmd; // TODO: use logic as to not write data for LEDs that have not changed - dash_mcu_state.bots_led = _data.LED[static_cast(DashLED_e::BOTS_LED)]; dash_mcu_state.launch_control_led = _data.LED[static_cast(DashLED_e::LAUNCH_CONTROL_LED)]; dash_mcu_state.mode_led = _data.LED[static_cast(DashLED_e::MODE_LED)]; dash_mcu_state.mechanical_brake_led = _data.LED[static_cast(DashLED_e::MECH_BRAKE_LED)]; @@ -40,15 +39,17 @@ CAN_message_t DashboardInterface::write() dash_mcu_state.inertia_status_led = _data.LED[static_cast(DashLED_e::INERTIA_LED)]; dash_mcu_state.start_status_led = _data.LED[static_cast(DashLED_e::START_LED)]; dash_mcu_state.motor_controller_error_led = _data.LED[static_cast(DashLED_e::MC_ERROR_LED)]; + + dash_mcu_state.bots_led = _data.LED[static_cast(DashLED_e::BOTS_LED)]; dash_mcu_state.imd_led = _data.LED[static_cast(DashLED_e::IMD_LED)]; dash_mcu_state.ams_led = _data.LED[static_cast(DashLED_e::AMS_LED)]; - + dash_mcu_state.glv_led = _data.LED[static_cast(DashLED_e::GLV_LED)]; dash_mcu_state.pack_charge_led = _data.LED[static_cast(DashLED_e::CRIT_CHARGE_LED)]; CAN_message_t can_msg; - can_msg.id = Pack_DASHBOARD_MCU_STATE_hytech(&dash_mcu_state, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); - + auto id = Pack_DASHBOARD_MCU_STATE_hytech(&dash_mcu_state, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + can_msg.id = id; // this circular buffer implementation requires that you push your data in a array buffer // all this does is put the msg into a uint8_t buffer and pushes it onto the queue uint8_t buf[sizeof(CAN_message_t)] = {}; @@ -66,14 +67,43 @@ void DashboardInterface::setLED(DashLED_e led, LEDColors_e color) _data.LED[static_cast(led)] = static_cast(color); } +void DashboardInterface::tick10(MCUInterface* mcu, int car_state, bool buzzer, bool drivetrain_error, TorqueLimit_e torque) +{ + + soundBuzzer(buzzer); + + setLED(DashLED_e::AMS_LED, mcu->bms_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); + setLED(DashLED_e::IMD_LED, mcu->imd_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); + setLED(DashLED_e::BOTS_LED, mcu->get_bots_ok() ? LEDColors_e::ON : LEDColors_e::RED); + setLED(DashLED_e::START_LED, car_state == int(MCU_STATE::READY_TO_DRIVE) ? LEDColors_e::ON : LEDColors_e::RED); + setLED(DashLED_e::MC_ERROR_LED, !drivetrain_error ? LEDColors_e::ON : LEDColors_e::RED); + setLED(DashLED_e::COCKPIT_BRB_LED, mcu->brb_ok_is_high() ? LEDColors_e::ON : LEDColors_e::RED); + + switch(torque){ + case TorqueLimit_e::TCMUX_LOW_TORQUE: + setLED(DashLED_e::MODE_LED, LEDColors_e::OFF); + break; + case TorqueLimit_e::TCMUX_MID_TORQUE: + setLED(DashLED_e::MODE_LED, LEDColors_e::YELLOW); + break; + case TorqueLimit_e::TCMUX_FULL_TORQUE: + setLED(DashLED_e::MODE_LED, LEDColors_e::ON); + break; + default: + setLED(DashLED_e::MODE_LED, LEDColors_e::RED); + break; + } + + write(); +} + DialMode_e DashboardInterface::getDialMode() {return _data.dial_mode;} bool DashboardInterface::startButtonPressed() {return _data.button.start;} bool DashboardInterface::specialButtonPressed() {return _data.button.mark;} -bool DashboardInterface::torqueButtonPressed() {return _data.button.mode;} +bool DashboardInterface::torqueModeButtonPressed() {return _data.button.torque_mode;} bool DashboardInterface::inverterResetButtonPressed() {return _data.button.mc_cycle;} bool DashboardInterface::launchControlButtonPressed() {return _data.button.launch_ctrl;} -bool DashboardInterface::torqueLoadingButtonPressed() {return _data.button.torque_mode;} bool DashboardInterface::nightModeButtonPressed() {return _data.button.led_dimmer;} bool DashboardInterface::leftShifterButtonPressed() {return _data.button.left_shifter;} bool DashboardInterface::rightShifterButtonPressed() {return _data.button.right_shifter;} diff --git a/lib/interfaces/src/MCUInterface.cpp b/lib/interfaces/src/MCUInterface.cpp index 0cf1a7d7d..075c8fd75 100644 --- a/lib/interfaces/src/MCUInterface.cpp +++ b/lib/interfaces/src/MCUInterface.cpp @@ -5,10 +5,15 @@ /* Initialize shutdown circuit input readings */ void MCUInterface::init() { + // Set pin mode + pinMode(pins_.pin_inv_en, OUTPUT); + pinMode(pins_.pin_inv_24V_en, OUTPUT); + pinMode(pins_.pin_brake_light_ctrl, OUTPUT); // Set initial shutdown circuit readings bms_ok_high = false; imd_ok_high = false; + brb_ok_high = false; // Enable inverters (?) // Should be called from drivetrain @@ -25,11 +30,14 @@ void MCUInterface::read_mcu_status() /* Measure shutdown circuits' input */ void MCUInterface::measure_shutdown_circuit_input() { - - bms_ok_high = digitalRead(pins_.pin_bms_ok_read); - imd_ok_high = digitalRead(pins_.pin_imd_ok_read); + // TODO: change these back to pins from constructor + bms_ok_high = digitalRead(DEFAULT_BMS_SENSE_PIN); + imd_ok_high = digitalRead(DEFAULT_IMD_SENSE_PIN); + bspd_ok_high = digitalRead(pins_.pin_bspd_ok_read); software_ok_high = digitalRead(pins_.pin_software_ok_read); + brb_ok_high = digitalRead(pins_.pin_brb_ok_read); + } /* Measure shutdown circuits' voltages */ @@ -40,6 +48,7 @@ void MCUInterface::measure_shutdown_circuit_voltage() shutdown_c_above_threshold = digitalRead(pins_.pin_imd_ok_read); shutdown_d_above_threshold = digitalRead(pins_.pin_bms_ok_read); shutdown_e_above_threshold = digitalRead(pins_.pin_bspd_ok_read); + shutdown_g_above_threshold = digitalRead(pins_.pin_brb_ok_read); } /* Write brake light */ @@ -71,6 +80,17 @@ bool MCUInterface::imd_ok_is_high() return imd_ok_high; } +// BRB +bool MCUInterface::brb_ok_is_high() +{ + return brb_ok_high; +} + +bool MCUInterface::get_bots_ok() +{ + return shutdown_b_above_threshold; +} + /* Send CAN message */ // MCU status void MCUInterface::enqueue_CAN_mcu_status() @@ -100,6 +120,7 @@ void MCUInterface::update_mcu_status_CAN() mcu_status_.set_shutdown_c_above_threshold(shutdown_c_above_threshold); mcu_status_.set_shutdown_d_above_threshold(shutdown_d_above_threshold); mcu_status_.set_shutdown_e_above_threshold(shutdown_e_above_threshold); + } // Main loop diff --git a/lib/interfaces/src/SABInterface.cpp b/lib/interfaces/src/SABInterface.cpp new file mode 100644 index 000000000..54d15d802 --- /dev/null +++ b/lib/interfaces/src/SABInterface.cpp @@ -0,0 +1,17 @@ +#include "SABInterface.h" + +void SABInterface::retrieve_pots_and_load_cells_CAN(CAN_message_t &recvd_msg) +{ + SAB_SUSPENSION_t sab_data; + Unpack_SAB_SUSPENSION_hytech(&sab_data, recvd_msg.buf, recvd_msg.len); + + rlLoadCell.lastSample = sab_data.load_cell_rl; + rrLoadCell.lastSample = sab_data.load_cell_rr; + pot3.lastSample = sab_data.potentiometer_rl; + pot4.lastSample = sab_data.potentiometer_rr; + + rlLoadCell.convert(); + rrLoadCell.convert(); + pot3.convert(); + pot4.convert(); +} \ No newline at end of file diff --git a/lib/interfaces/src/TelemetryInterface.cpp b/lib/interfaces/src/TelemetryInterface.cpp index c62814477..3b7403cb8 100644 --- a/lib/interfaces/src/TelemetryInterface.cpp +++ b/lib/interfaces/src/TelemetryInterface.cpp @@ -16,29 +16,17 @@ void TelemetryInterface::update_pedal_readings_CAN_msg(const AnalogConversion_s enqueue_CAN(mcu_pedal_readings_, ID_MCU_PEDAL_READINGS); } // MCP3204 returns structure -void TelemetryInterface::update_load_cells_CAN_msg(const AnalogConversion_s &lc_fl, - const AnalogConversion_s &lc_fr) { - // do sth with mcu_load_cells_ - mcu_load_cells_.set_FL_load_cell(lc_fl.raw); - mcu_load_cells_.set_FR_load_cell(lc_fr.raw); - // mcu_load_cells_.set_RL_load_cell(lc_fl.raw); - // mcu_load_cells_.set_RR_load_cell(lc_fr.raw); - - enqueue_CAN(mcu_load_cells_, ID_MCU_LOAD_CELLS); -} -// MCP3204 returns structure -void TelemetryInterface::update_potentiometers_CAN_msg(const AnalogConversion_s &pots_fl, - const AnalogConversion_s &pots_fr) { - // do sth with mcu_front_potentiometers_ - mcu_front_potentiometers_.set_pot1(pots_fl.raw); - mcu_front_potentiometers_.set_pot3(pots_fr.raw); - - // do sth with mcu_rear_potentiometers_ - // mcu_rear_potentiometers_.set_pot4(pots_rl.raw); - // mcu_rear_potentiometers_.set_pot6(pots_rr.raw); - - enqueue_CAN(mcu_front_potentiometers_, ID_MCU_FRONT_POTS); - // enqueue_CAN(mcu_rear_potentiometers_, ID_MCU_REAR_POTS); +void TelemetryInterface::update_suspension_CAN_msg(const AnalogConversion_s &lc_fl, + const AnalogConversion_s &lc_fr, + const AnalogConversion_s &pots_fl, + const AnalogConversion_s &pots_fr) { + MCU_SUSPENSION_t sus; + sus.load_cell_fl = lc_fl.raw; + sus.load_cell_fr = lc_fr.raw; + sus.potentiometer_fl = pots_fr.raw; + sus.potentiometer_fr = pots_fl.raw; + + enqueue_new_CAN(&sus, &Pack_MCU_SUSPENSION_hytech); } // SteeringDual and MCP3208 return structures void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderConversion_s &steer1, @@ -47,14 +35,130 @@ void TelemetryInterface::update_analog_readings_CAN_msg(const SteeringEncoderCon const AnalogConversion_s &reference, const AnalogConversion_s &glv) { // do sth with mcu_analog_readings_ - mcu_analog_readings_.set_steering_1(static_cast(steer1.angle * FIXED_POINT_PRECISION)); + mcu_analog_readings_.set_steering_1(steer1.angle); mcu_analog_readings_.set_steering_2(steer2.raw); mcu_analog_readings_.set_hall_effect_current(current.raw - reference.raw); + // Serial.println("hall effect current: "); + // Serial.println(mcu_analog_readings_.get_hall_effect_current()); mcu_analog_readings_.set_glv_battery_voltage(glv.raw); enqueue_CAN(mcu_analog_readings_, ID_MCU_ANALOG_READINGS); } +void TelemetryInterface::update_drivetrain_rpms_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) +{ + DRIVETRAIN_RPMS_TELEM_t rpms; + rpms.fl_motor_rpm = fl->get_speed(); + rpms.fr_motor_rpm = fr->get_speed(); + rpms.rl_motor_rpm = rl->get_speed(); + rpms.rr_motor_rpm = rr->get_speed(); + + enqueue_new_CAN(&rpms, &Pack_DRIVETRAIN_RPMS_TELEM_hytech); +} + +void TelemetryInterface::update_drivetrain_err_status_CAN_msg(InvInt_t* fl, InvInt_t* fr, InvInt_t* rl, InvInt_t* rr) +{ + + if (1) { // We should only write error status if some error has occurred, but for now, this is just an if(1) + DRIVETRAIN_ERR_STATUS_TELEM_t errors; + errors.mc1_diagnostic_number = fl->get_error_status(); + errors.mc2_diagnostic_number = fr->get_error_status(); + errors.mc3_diagnostic_number = rl->get_error_status(); + errors.mc4_diagnostic_number = rr->get_error_status(); + enqueue_new_CAN(&errors, &Pack_DRIVETRAIN_ERR_STATUS_TELEM_hytech); + } + +} +//Pack_DRIVETRAIN_STATUS_TELEM_hytech +void TelemetryInterface::update_drivetrain_status_telem_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr, + bool accel_implaus, + bool brake_implaus, + float accel_per, + float brake_per) +{ + DRIVETRAIN_STATUS_TELEM_t status; + + status.mc1_dc_on = fl->get_dc_on(); + status.mc1_derating_on = fl->get_derating_on(); + status.mc1_error = fl->get_error(); + status.mc1_inverter_on = fl->get_inverter_on(); + status.mc1_quit_dc = fl->get_quit_dc_on(); + status.mc1_quit_inverter_on = fl->get_quit_inverter_on(); + status.mc1_system_ready = fl->inverter_system_ready(); + status.mc1_warning = fl->get_warning(); + + status.mc2_dc_on = fr->get_dc_on(); + status.mc2_derating_on = fr->get_derating_on(); + status.mc2_error = fr->get_error(); + status.mc2_inverter_on = fr->get_inverter_on(); + status.mc2_quit_dc = fr->get_quit_dc_on(); + status.mc2_quit_inverter_on = fr->get_quit_inverter_on(); + status.mc2_system_ready = fr->inverter_system_ready(); + status.mc2_warning = fr->get_warning(); + + status.mc3_dc_on = rl->get_dc_on(); + status.mc3_derating_on = rl->get_derating_on(); + status.mc3_error = rl->get_error(); + status.mc3_inverter_on = rl->get_inverter_on(); + status.mc3_quit_dc = rl->get_quit_dc_on(); + status.mc3_quit_inverter_on = rl->get_quit_inverter_on(); + status.mc3_system_ready = rl->inverter_system_ready(); + status.mc3_warning = rl->get_warning(); + + status.mc4_dc_on = rr->get_dc_on(); + status.mc4_derating_on = rr->get_derating_on(); + status.mc4_error = rr->get_error(); + status.mc4_inverter_on = rr->get_inverter_on(); + status.mc4_quit_dc = rr->get_quit_dc_on(); + status.mc4_quit_inverter_on = rr->get_quit_inverter_on(); + status.mc4_system_ready = rr->inverter_system_ready(); + status.mc4_warning = rr->get_warning(); + + status.accel_implausible = accel_implaus; + status.brake_implausible = brake_implaus; + status.brake_percent = (uint8_t)(abs(brake_per) * 100); + status.accel_percent = (uint8_t)(abs(accel_per) * 100); + + enqueue_new_CAN(&status, &Pack_DRIVETRAIN_STATUS_TELEM_hytech); +} + +void TelemetryInterface::update_drivetrain_torque_telem_CAN_msg( + InvInt_t* fl, + InvInt_t* fr, + InvInt_t* rl, + InvInt_t* rr) +{ + // TODO: change this to use actual torque values from inverter + // Torque current just temporary for gearbox seal validation + DRIVETRAIN_TORQUE_TELEM_t torque; + torque.fl_motor_torque = fl->get_torque_current(); + torque.fr_motor_torque = fr->get_torque_current(); + torque.rl_motor_torque = rl->get_torque_current(); + torque.rr_motor_torque = rr->get_torque_current(); + + // Serial.printf("TORQUE:\nFL: %d\nFR: %d\nRL: %d\nRR: %d\n", + // torque.fl_motor_torque, + // torque.fr_motor_torque, + // torque.rl_motor_torque, + // torque.rr_motor_torque); + + enqueue_new_CAN(&torque, &Pack_DRIVETRAIN_TORQUE_TELEM_hytech); +} + +// Pack_PENTHOUSE_ACCUM_MSG_hytech +void TelemetryInterface::update_penthouse_accum_CAN_msg(const AnalogConversion_s ¤t, const AnalogConversion_s &reference) +{ + PENTHOUSE_ACCUM_MSG_t message; + message.hall_curr_ref = reference.raw; + message.hall_curr_signal = current.raw; + + enqueue_new_CAN(&message, &Pack_PENTHOUSE_ACCUM_MSG_hytech); +} + /* Send CAN messages */ template void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { @@ -70,11 +174,30 @@ void TelemetryInterface::enqueue_CAN(T msg_class, uint32_t id) { } +/* Send inverter CAN messages with new CAN library */ +template +void TelemetryInterface::enqueue_new_CAN(U* structure, uint32_t (* pack_function)(U*, uint8_t*, uint8_t*, uint8_t*)) { + CAN_message_t can_msg; + can_msg.id = pack_function(structure, can_msg.buf, &can_msg.len, (uint8_t*) &can_msg.flags.extended); + uint8_t buf[sizeof(CAN_message_t)] = {}; + memmove(buf, &can_msg, sizeof(CAN_message_t)); + msg_queue_->push_back(buf, sizeof(CAN_message_t)); +} + + /* Tick SysClock */ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, const AnalogConversionPacket_s<4> &adc2, const AnalogConversionPacket_s<4> &adc3, - const SteeringEncoderConversion_s &encoder) { + const SteeringEncoderConversion_s &encoder, + const InvInt_t* fl, + const InvInt_t* fr, + const InvInt_t* rl, + const InvInt_t* rr, + bool accel_implaus, + bool brake_implaus, + float accel_per, + float brake_per) { // Pedals update_pedal_readings_CAN_msg(adc1.conversions[channels_.accel1_channel], @@ -90,13 +213,19 @@ void TelemetryInterface::tick(const AnalogConversionPacket_s<8> &adc1, adc1.conversions[channels_.glv_sense_channel]); // enqueue_CAN_mcu_analog_readings(); // Load cells - update_load_cells_CAN_msg(adc2.conversions[channels_.loadcell_fl_channel], - adc3.conversions[channels_.loadcell_fr_channel]); - // enqueue_CAN_mcu_load_cells(); - // Pots - update_potentiometers_CAN_msg(adc2.conversions[channels_.pots_fl_channel], - adc3.conversions[channels_.pots_fr_channel]); + update_suspension_CAN_msg(adc2.conversions[channels_.loadcell_fl_channel], + adc3.conversions[channels_.loadcell_fr_channel], + adc2.conversions[channels_.pots_fl_channel], + adc3.conversions[channels_.pots_fr_channel]); + + update_drivetrain_rpms_CAN_msg(fl, fr, rl, rr); + update_drivetrain_err_status_CAN_msg(fl, fr, rl, rr); + update_drivetrain_status_telem_CAN_msg(fl, fr, rl, rr, accel_implaus, brake_implaus, accel_per, brake_per); + update_drivetrain_torque_telem_CAN_msg(fl, fr, rl, rr); + + update_penthouse_accum_CAN_msg(adc1.conversions[channels_.current_channel], + adc1.conversions[channels_.current_ref_channel]); // enqueue_CAN_mcu_front_potentiometers(); // enqueue_CAN_mcu_rear_potentiometers(); -} +} \ No newline at end of file diff --git a/lib/interfaces/src/WatchdogInterface.cpp b/lib/interfaces/src/WatchdogInterface.cpp index 9a95aaa1f..80041664e 100644 --- a/lib/interfaces/src/WatchdogInterface.cpp +++ b/lib/interfaces/src/WatchdogInterface.cpp @@ -2,6 +2,8 @@ /* Pin mode output to watchdog WD */ void WatchdogInterface::init(unsigned long curr_millis) { + // Set pin mode + pinMode(pin_watchdog_input_, OUTPUT); watchdog_time = curr_millis; set_watchdog_state(HIGH); @@ -17,11 +19,13 @@ void WatchdogInterface::set_start_state() { /* Toggle watchdog WD to kick dog */ void WatchdogInterface::kick_watchdog(unsigned long curr_millis) { + if ((curr_millis - watchdog_time) > WATCHDOG_KICK_INTERVAL) { watchdog_state = !watchdog_state; digitalWrite(pin_watchdog_input_, watchdog_state); watchdog_time = curr_millis; } + } /* Get interface status */ diff --git a/lib/mock_interfaces/DashboardInterface.h b/lib/mock_interfaces/DashboardInterface.h index cece587e1..e9cd3ba05 100644 --- a/lib/mock_interfaces/DashboardInterface.h +++ b/lib/mock_interfaces/DashboardInterface.h @@ -1,5 +1,19 @@ #ifndef DASHBOARDINTERFACE #define DASHBOARDINTERFACE + +/* + Enum for the car's torque limits + MOVE ME! - ideally into a TorqueControllerDefs.h file + to prevent circular dependencies +*/ +enum class TorqueLimit_e +{ + TCMUX_LOW_TORQUE = 0, + TCMUX_MID_TORQUE = 1, + TCMUX_FULL_TORQUE = 2, + TCMUX_NUM_TORQUE_LIMITS = 3, +}; + /* Enum for the modes on the dial, corresponds directly to dial index pos. */ enum class DialMode_e { @@ -47,6 +61,7 @@ class DashboardInterface public: bool start_button_status_; bool startButtonPressed() { return start_button_status_; }; + bool checkBuzzer(){ return false; }; }; #endif /* DASHBOARDINTERFACE */ diff --git a/lib/mock_interfaces/WatchdogInterface.h b/lib/mock_interfaces/WatchdogInterface.h index 1f712c583..4551c3e2a 100644 --- a/lib/mock_interfaces/WatchdogInterface.h +++ b/lib/mock_interfaces/WatchdogInterface.h @@ -2,7 +2,7 @@ #define __WATCHDOG_INTERFACE_H__ #include "SysClock.h" -const unsigned long WATCHDOG_KICK_INTERVAL = 7; // milliseconds +const unsigned long WATCHDOG_KICK_INTERVAL = 10; // milliseconds class WatchdogInterface { diff --git a/lib/state_machine/include/MCUStateMachine.h b/lib/state_machine/include/MCUStateMachine.h index 7fca8a782..a74f76d6d 100644 --- a/lib/state_machine/include/MCUStateMachine.h +++ b/lib/state_machine/include/MCUStateMachine.h @@ -11,6 +11,7 @@ #include "SafetySystem.h" #include "DashboardInterface.h" #include "AMSInterface.h" + // #include "IMDInterface.h" enum class CAR_STATE @@ -48,6 +49,7 @@ class MCUStateMachine // void tick_state_machine(const SysTick_s &tick); void tick_state_machine(unsigned long cm); CAR_STATE get_state() { return current_state_; } + bool car_in_ready_to_drive() { return current_state_ == CAR_STATE::READY_TO_DRIVE; }; private: void set_state_(CAR_STATE new_state, unsigned long curr_time); @@ -71,6 +73,8 @@ class MCUStateMachine // IMDInterface *imd_; SafetySystem *safety_system_; TorqueControllerMux *controller_mux_; + + }; #include "MCUStateMachine.tpp" #endif /* MCUSTATEMACHINE */ diff --git a/lib/state_machine/include/MCUStateMachine.tpp b/lib/state_machine/include/MCUStateMachine.tpp index 1eb14f06b..cefa7d3e8 100644 --- a/lib/state_machine/include/MCUStateMachine.tpp +++ b/lib/state_machine/include/MCUStateMachine.tpp @@ -6,12 +6,49 @@ void MCUStateMachine::tick_state_machine(unsigned long curren switch (get_state()) { case CAR_STATE::STARTUP: + + hal_println("in startup state"); set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); break; case CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: { + // Serial.println("eror in ts not active:"); + // Serial.println(safety_system_->get_software_is_ok()); + // Serial.println(); + // hal_println("tractive system not active state"); + + // auto data = pedals_->getPedalsSystemData(); + // auto mux_test = controller_mux_->getDrivetrainCommand(); + + // hal_println("speeds 1 through 4"); + // Serial.println(mux_test.speeds_rpm[0]); + // Serial.println(mux_test.speeds_rpm[1]); + // Serial.println(mux_test.speeds_rpm[2]); + // Serial.println(mux_test.speeds_rpm[3]); + + // hal_println("torqeus"); + // Serial.println(mux_test.torqueSetpoints[0]); + // Serial.println(mux_test.torqueSetpoints[1]); + // Serial.println(mux_test.torqueSetpoints[2]); + // Serial.println(mux_test.torqueSetpoints[3]); + // Serial.println(); + // Serial.print(data.brakeImplausible); + // Serial.print(" "); + // Serial.print(data.accelImplausible); + // Serial.print(" "); + // Serial.print(data.brakeAndAccelPressedImplausibility); + // Serial.print(" "); + // Serial.print(data.implausibilityExceededMaxDuration); + // Serial.println(); + // Serial.print(data.accelPercent); + // Serial.print(" "); + // Serial.print(data.brakePercent); + // Serial.print(" "); + + // if TS is above HV threshold, move to Tractive System Active + drivetrain_->disable_no_pins(); if (drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); @@ -21,8 +58,14 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::TRACTIVE_SYSTEM_ACTIVE: { + if (buzzer_->buzzer_is_on()) + { + buzzer_->deactivate(); + } + // hal_println("in tractive system active state"); // TODO migrate to new pedals system auto data = pedals_->getPedalsSystemData(); + drivetrain_->disable_no_pins(); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); @@ -38,6 +81,8 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::ENABLING_INVERTERS: { + + // hal_println("in enabling inverters state"); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { set_state_(CAR_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE, current_millis); @@ -55,6 +100,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren { // TODO handle the drivetrain state change back to startup phase 1 and/or move this into // the drivetrain state machine handling + if (!drivetrain_->hv_over_threshold_on_drivetrain()) { hal_println("drivetrain not over thresh in WRTD?"); @@ -62,8 +108,10 @@ void MCUStateMachine::tick_state_machine(unsigned long curren break; } + drivetrain_->command_drivetrain_no_torque(); + // if the ready to drive sound has been playing for long enough, move to ready to drive mode - if (buzzer_->done(current_millis)) + if (buzzer_->done(current_millis) && !dashboard_->checkBuzzer()) { set_state_(CAR_STATE::READY_TO_DRIVE, current_millis); } @@ -73,6 +121,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren case CAR_STATE::READY_TO_DRIVE: { auto data = pedals_->getPedalsSystemData(); + // auto test = controller_mux_->getDrivetrainCommand(); if (!drivetrain_->hv_over_threshold_on_drivetrain()) { @@ -83,6 +132,7 @@ void MCUStateMachine::tick_state_machine(unsigned long curren if (drivetrain_->drivetrain_error_occured()) { + set_state_(CAR_STATE::TRACTIVE_SYSTEM_ACTIVE, current_millis); break; } @@ -91,15 +141,10 @@ void MCUStateMachine::tick_state_machine(unsigned long curren { drivetrain_->command_drivetrain(controller_mux_->getDrivetrainCommand()); } - else + else { drivetrain_->command_drivetrain_no_torque(); - hal_println("not calculating torque"); - // hal_printf("no brake implausibility: %d\n", pedals_data.brakeImplausible); - // hal_printf("no accel implausibility: %d\n", pedals_data.accelImplausible); - // hal_printf("bms heartbeat: %d\n", bms_->heartbeat_check(current_millis)); - // hal_printf("get bms ok high: %d\n", bms_->ok_high()); - // hal_printf("get imd ok high: %d\n", imd_->ok_high()); + } break; @@ -110,14 +155,14 @@ void MCUStateMachine::tick_state_machine(unsigned long curren template void MCUStateMachine::set_state_(CAR_STATE new_state, unsigned long curr_time) { - hal_println("running exit logic"); + // hal_println("running exit logic"); - hal_printf("\n to state: %i\n", new_state); + // hal_printf("\n to state: %i\n", new_state); handle_exit_logic_(current_state_, curr_time); current_state_ = new_state; - hal_println("running entry logic"); + // hal_println("running entry logic"); handle_entry_logic_(new_state, curr_time); } @@ -138,7 +183,7 @@ void MCUStateMachine::handle_exit_logic_(CAR_STATE prev_state break; case CAR_STATE::READY_TO_DRIVE: { - drivetrain_->disable(); + // drivetrain_->disable(); break; } } @@ -163,13 +208,13 @@ void MCUStateMachine::handle_entry_logic_(CAR_STATE new_state { // make dashboard sound buzzer buzzer_->activate_buzzer(curr_time); - hal_println("RTDS enabled"); + // hal_println("RTDS enabled"); break; } case CAR_STATE::READY_TO_DRIVE: { - hal_printf("%i\n", curr_time); - hal_println("Ready to drive"); + // hal_printf("%i\n", curr_time); + // hal_println("Ready to drive"); break; } } diff --git a/lib/systems/include/Buzzer.h b/lib/systems/include/Buzzer.h index 26c4a9394..36aeaed0b 100644 --- a/lib/systems/include/Buzzer.h +++ b/lib/systems/include/Buzzer.h @@ -1,6 +1,7 @@ #ifndef BUZZER #define BUZZER + class BuzzerController { @@ -8,14 +9,19 @@ class BuzzerController BuzzerController(int duration_of_activation_ms) { buzzer_period_ = duration_of_activation_ms; } - + void deactivate(); void activate_buzzer(unsigned long act_time); bool done(unsigned long curr_time); - bool buzzer_is_on() { return buzzer_on_; } + bool buzzer_is_on() { + if(buzzer_on_){ + // Serial.println("buzzer should be on now"); + } + return buzzer_on_; + } private: - unsigned int buzzer_period_; + unsigned long buzzer_period_; unsigned long last_activation_time_; bool buzzer_on_; }; diff --git a/lib/systems/include/DrivetrainSystem.h b/lib/systems/include/DrivetrainSystem.h index b8c502c37..fbd6adfec 100644 --- a/lib/systems/include/DrivetrainSystem.h +++ b/lib/systems/include/DrivetrainSystem.h @@ -10,7 +10,7 @@ struct DrivetrainCommand_s { float speeds_rpm[NUM_MOTORS]; - float torqueSetpoints[NUM_MOTORS]; + float torqueSetpoints[NUM_MOTORS]; // FIXME: misnomer. This represents the magnitude of the torque the inverter can command to reach the commanded speed setpoint }; struct DrivetrainDynamicReport_s @@ -28,7 +28,7 @@ class DrivetrainSystem public: /// @brief order of array: 0: FL, 1: FR, 2: RL, 3: RR /// @param inverters inverter pointers - DrivetrainSystem(const std::array &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 25) + DrivetrainSystem(const std::array &inverters, MCUInterface *mcu_interface, int init_time_limit_ms, uint16_t min_hv_voltage = 60, int min_cmd_period_ms = 1) : inverters_(inverters), init_time_limit_ms_(init_time_limit_ms), min_hv_voltage_(min_hv_voltage), min_cmd_period_(min_cmd_period_ms) { // values from: https://www.amk-motion.com/amk-dokucd/dokucd/en/content/resources/pdf-dateien/fse/motor_data_sheet_a2370dd_dd5.pdf @@ -58,11 +58,13 @@ class DrivetrainSystem bool handle_inverter_startup(unsigned long curr_time); // on entry logic void command_drivetrain_no_torque(); + void command_drivetrain_debug(); // check to see if init time limit has passed bool inverter_init_timeout(unsigned long curr_time); bool hv_over_threshold_on_drivetrain(); void disable(); + void disable_no_pins(); bool drivetrain_error_occured(); void reset_drivetrain(); void command_drivetrain(const DrivetrainCommand_s &data); diff --git a/lib/systems/include/DrivetrainSystem.tpp b/lib/systems/include/DrivetrainSystem.tpp index e7c384429..7da5cb459 100644 --- a/lib/systems/include/DrivetrainSystem.tpp +++ b/lib/systems/include/DrivetrainSystem.tpp @@ -18,16 +18,22 @@ bool DrivetrainSystem::inverter_init_timeout(unsigned long curr_ti template bool DrivetrainSystem::handle_inverter_startup(unsigned long curr_time) { - if (drivetrain_ready_() && !hv_en_requested_) + // 1. if system ready + + if (drivetrain_ready_() && !check_drivetrain_quit_dc_on_() && !drivetrain_enabled_()) { + + // Serial.println("drivetrain ready and enabling drivetrain hv"); enable_drivetrain_hv_(curr_time); - hv_en_requested_ = true; + + // hv_en_requested_ = true; return false; } - else if (drivetrain_ready_() && check_drivetrain_quit_dc_on_() && !enable_requested_ && hv_en_requested_) + else if (drivetrain_ready_() && check_drivetrain_quit_dc_on_() && !drivetrain_enabled_()) { + request_enable_(); - enable_requested_ = true; + // enable_requested_ = true; return false; } bool all_ready = (drivetrain_ready_() && check_drivetrain_quit_dc_on_() && drivetrain_enabled_()); @@ -47,7 +53,7 @@ void DrivetrainSystem::enable_drivetrain_hv_(unsigned long curr_ti template void DrivetrainSystem::request_enable_() { - mcu_interface_->enable_inverters_pin(); + // mcu_interface_->enable_inverters_pin(); for (auto inv_pointer : inverters_) { inv_pointer->request_enable_inverter(); @@ -57,81 +63,96 @@ void DrivetrainSystem::request_enable_() // rate limited commands. we will only be commanding one of these at a time. /*----------------------------------------------------------------------------------------*/ template -void DrivetrainSystem::command_drivetrain_no_torque() +void DrivetrainSystem::command_drivetrain_debug() { - if ((curr_system_millis_ - last_no_torque_cmd_time_) > min_cmd_period_) - { + // if ((curr_system_millis_ - last_no_torque_cmd_time_) > min_cmd_period_) + // { for (auto inv_pointer : inverters_) { - inv_pointer->command_no_torque(); + inv_pointer->command_debug(); } last_no_torque_cmd_time_ = curr_system_millis_; - } + // } } template -void DrivetrainSystem::enable_drivetrain_reset() +void DrivetrainSystem::command_drivetrain_no_torque() { - reset_requested_ = true; - last_reset_pressed_time_ = curr_system_millis_; + // if ((curr_system_millis_ - last_no_torque_cmd_time_) > min_cmd_period_) + // { + for (auto inv_pointer : inverters_) + { + inv_pointer->command_no_torque(); + } + last_no_torque_cmd_time_ = curr_system_millis_; + // } } + + template void DrivetrainSystem::check_reset_condition() { - if ((curr_system_millis_ - last_reset_pressed_time_) > reset_interval_) - { - reset_requested_ = false; - } + // if ((curr_system_millis_ - last_reset_pressed_time_) > reset_interval_) + // { + reset_requested_ = false; + // } } template void DrivetrainSystem::reset_drivetrain() { - // Handle reset condition - if (reset_requested_) - { - // Handle CAN send rate - if ((curr_system_millis_ - last_reset_cmd_time_) > min_cmd_period_) + + for (auto inv_pointer : inverters_) { - for (auto inv_pointer : inverters_) - { - inv_pointer->command_reset(); - } - last_reset_cmd_time_ = curr_system_millis_; - } - } + inv_pointer->command_reset(); + } + +} + +template +void DrivetrainSystem::disable_no_pins() +{ + // if ((curr_system_millis_ - last_disable_cmd_time_) > min_cmd_period_) + // { + for (auto inv_pointer : inverters_) + { + inv_pointer->disable(); + } + last_disable_cmd_time_ = curr_system_millis_; + // } } template void DrivetrainSystem::disable() { - if ((curr_system_millis_ - last_disable_cmd_time_) > min_cmd_period_) - { + // if ((curr_system_millis_ - last_disable_cmd_time_) > min_cmd_period_) + // { for (auto inv_pointer : inverters_) { inv_pointer->disable(); } last_disable_cmd_time_ = curr_system_millis_; - } + // } mcu_interface_->disable_inverters_pin(); + mcu_interface_->enable_inverters_pin(); } template void DrivetrainSystem::command_drivetrain(const DrivetrainCommand_s &data) { - if ((curr_system_millis_ - last_general_cmd_time_) > min_cmd_period_) - { + // if ((curr_system_millis_ - last_general_cmd_time_) > min_cmd_period_) + // { int index = 0; for (auto inv_pointer : inverters_) { inv_pointer->handle_command({data.torqueSetpoints[index], data.speeds_rpm[index]}); index++; } - last_general_cmd_time_ = curr_system_millis_; - } + // last_general_cmd_time_ = curr_system_millis_; + // } } /*----------------------------------------------------------------------------------------*/ // feedback functions @@ -141,7 +162,7 @@ bool DrivetrainSystem::drivetrain_error_occured() { for (auto inv_pointer : inverters_) { - if (inv_pointer->error()) + if (inv_pointer->get_error()) { return true; } @@ -177,9 +198,10 @@ bool DrivetrainSystem::drivetrain_ready_() template bool DrivetrainSystem::check_drivetrain_quit_dc_on_() { + // return (inverters_[3]->dc_quit_on()); for (auto inv_pointer : inverters_) { - if (!inv_pointer->dc_quit_on()) + if (!inv_pointer->get_quit_dc_on()) { return false; } @@ -191,9 +213,10 @@ template bool DrivetrainSystem::drivetrain_enabled_() { + // return (inverters_[3]->quit_inverter_on()); for (auto inv_pointer : inverters_) { - if (!inv_pointer->quit_inverter_on()) + if (!inv_pointer->get_quit_inverter_on()) { return false; } diff --git a/lib/systems/include/PedalsSystem.h b/lib/systems/include/PedalsSystem.h index 3b46646a2..63569a0c0 100644 --- a/lib/systems/include/PedalsSystem.h +++ b/lib/systems/include/PedalsSystem.h @@ -8,14 +8,15 @@ struct PedalsSystemData_s { - bool accelImplausible:1; - bool brakeImplausible:1; - bool brakePressed:1; - bool mechBrakeActive:1; - bool brakeAndAccelPressedImplausibility:1; - bool implausibilityExceededMaxDuration:1; + bool accelImplausible : 1; + bool brakeImplausible : 1; + bool brakePressed : 1; + bool mechBrakeActive : 1; + bool brakeAndAccelPressedImplausibility : 1; + bool implausibilityExceededMaxDuration : 1; float accelPercent; float brakePercent; + float regenPercent; }; /// @brief Pedals params struct that will hold min / max that will be used for evaluateion. @@ -31,7 +32,7 @@ struct PedalsParams class PedalsSystem { -public: +public: PedalsSystem(const PedalsParams &accelParams, const PedalsParams &brakeParams, float mechBrakeActiveThreshold) { accelParams_ = accelParams; @@ -45,7 +46,12 @@ class PedalsSystem { return data_; } - + + PedalsSystemData_s getPedalsSystemDataCopy() + { + return data_; + } + void tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, @@ -60,7 +66,7 @@ class PedalsSystem private: PedalsSystemData_s data_; - + float remove_deadzone_(float conversion_input, float deadzone); bool max_duration_of_implausibility_exceeded_(unsigned long curr_time); bool evaluate_pedal_implausibilities_(const AnalogConversion_s &pedalData1, const AnalogConversion_s &pedalData2, diff --git a/lib/systems/include/SafetySystem.h b/lib/systems/include/SafetySystem.h index 3e72017ad..f1a04d4b1 100644 --- a/lib/systems/include/SafetySystem.h +++ b/lib/systems/include/SafetySystem.h @@ -10,8 +10,9 @@ class SafetySystem public: SafetySystem(AMSInterface *ams, WatchdogInterface *wd): ams_(ams), - wd_(wd) {}; + wd_(wd){}; + /* Initialization */ void init(); diff --git a/lib/systems/include/TorqueControllerMux.h b/lib/systems/include/TorqueControllerMux.h index 3eee61531..4f38941e6 100644 --- a/lib/systems/include/TorqueControllerMux.h +++ b/lib/systems/include/TorqueControllerMux.h @@ -12,14 +12,7 @@ const float MAX_SPEED_FOR_MODE_CHANGE = 5.0; // m/s const float MAX_TORQUE_DELTA_FOR_MODE_CHANGE = 0.5; // Nm -enum class TorqueLimit_e -{ - TCMUX_LOW_TORQUE = 0, - TCMUX_MID_TORQUE = 1, - TCMUX_FULL_TORQUE = 2, - TCMUX_NUM_TORQUE_LIMITS = 3, -}; - +/// @brief class TorqueControllerMux { private: @@ -27,7 +20,7 @@ class TorqueControllerMux std::unordered_map dialModeMap_ = { {DialMode_e::MODE_0, TorqueController_e::TC_SAFE_MODE}, {DialMode_e::MODE_1, TorqueController_e::TC_SAFE_MODE}, - {DialMode_e::MODE_2, TorqueController_e::TC_NO_CONTROLLER}, + {DialMode_e::MODE_2, TorqueController_e::TC_LOAD_CELL_VECTORING}, {DialMode_e::MODE_3, TorqueController_e::TC_NO_CONTROLLER}, {DialMode_e::MODE_4, TorqueController_e::TC_NO_CONTROLLER}, {DialMode_e::MODE_5, TorqueController_e::TC_NO_CONTROLLER}, @@ -37,19 +30,30 @@ class TorqueControllerMux {TorqueLimit_e::TCMUX_MID_TORQUE, 15.0}, {TorqueLimit_e::TCMUX_FULL_TORQUE, 20.0} }; - DrivetrainCommand_s controllerCommands_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; + TorqueControllerOutput_s controllerOutputs_[static_cast(TorqueController_e::TC_NUM_CONTROLLERS)]; TorqueControllerNone torqueControllerNone_; TorqueControllerSimple torqueControllerSimple_; + TorqueControllerLoadCellVectoring torqueControllerLoadCellVectoring_; TorqueController_e muxMode_ = TorqueController_e::TC_NO_CONTROLLER; DrivetrainCommand_s drivetrainCommand_; TorqueLimit_e torqueLimit_ = TorqueLimit_e::TCMUX_LOW_TORQUE; bool torqueLimitButtonPressed_ = false; unsigned long torqueLimitButtonPressedTime_ = 0; public: -// Constructors + /// @brief torque controller mux in which default instances of all torque controllers are created for use TorqueControllerMux() - : torqueControllerNone_(controllerCommands_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) - , torqueControllerSimple_(controllerCommands_[static_cast(TorqueController_e::TC_SAFE_MODE)]) {} + : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) + , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)]) + , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) {} + + + /// @brief torque controller mux constructor that leaves all other TCs with defaults accept for simple TC + /// @param simpleTCRearTorqueScale the scaling from 0 to 2 in which 2 is full rear torque allocation, 0 is full front, 1 = balanced + /// @param simpleTCRegenTorqueScale scaling from 0 to 2 in which 0 is full rear regen and 2 is full front regen, 1 = balanced + TorqueControllerMux(float simpleTCRearTorqueScale, float simpleTCRegenTorqueScale) + : torqueControllerNone_(controllerOutputs_[static_cast(TorqueController_e::TC_NO_CONTROLLER)]) + , torqueControllerSimple_(controllerOutputs_[static_cast(TorqueController_e::TC_SAFE_MODE)], simpleTCRearTorqueScale, simpleTCRegenTorqueScale) + , torqueControllerLoadCellVectoring_(controllerOutputs_[static_cast(TorqueController_e::TC_LOAD_CELL_VECTORING)]) {} // Functions void tick( const SysTick_s& tick, diff --git a/lib/systems/include/TorqueControllers.h b/lib/systems/include/TorqueControllers.h index f9ad46215..6c07b4e74 100644 --- a/lib/systems/include/TorqueControllers.h +++ b/lib/systems/include/TorqueControllers.h @@ -7,58 +7,87 @@ #include #include "AnalogSensorsInterface.h" #include "DashboardInterface.h" +#include "PhysicalParameters.h" -const float AMK_MAX_RPM = 20000.0; +const float AMK_MAX_RPM = 20000; +// 10MPH LIMIT for lot testing lmao +// const float AMK_MAX_RPM = (13.4 * METERS_PER_SECOND_TO_RPM); // 30mph +// const float AMK_MAX_RPM = (4.47 * METERS_PER_SECOND_TO_RPM); // 10mph +// const float AMK_MAX_RPM = (2.235 * METERS_PER_SECOND_TO_RPM); // 5mph +// const float AMK_MAX_RPM = (.89 * METERS_PER_SECOND_TO_RPM); // 1mph +// const float const float AMK_MAX_TORQUE = 20.0; // TODO: update this with the true value const float MAX_REGEN_TORQUE = 10.0; +const DrivetrainCommand_s TC_COMMAND_NO_TORQUE = { + .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, + .torqueSetpoints = {0.0, 0.0, 0.0, 0.0}}; + +struct TorqueControllerOutput_s +{ + DrivetrainCommand_s command; + bool ready; +}; + enum TorqueController_e { TC_NO_CONTROLLER = 0, TC_SAFE_MODE = 1, - TC_NUM_CONTROLLERS = 2, + TC_LOAD_CELL_VECTORING = 2, + TC_NUM_CONTROLLERS = 3, }; /// @brief If a command fed through this function exceeds the specified power limit, all torques will be scaled down equally -/// @param command -/// @param drivetrainData +/// @param command +/// @param drivetrainData /// @param powerLimit In watts, not kilowatts -/// @param +/// @param /// @return A scaled down DrivetrainCommand_s static DrivetrainCommand_s TCPowerLimitScaleDown( DrivetrainCommand_s command, - DrivetrainDynamicReport_s* drivetrainData, - float powerLimit -); + DrivetrainDynamicReport_s *drivetrainData, + float powerLimit); /// @brief Apply a per-wheel torque limit -/// @param command -/// @param torqueLimits -/// @param +/// @param command +/// @param torqueLimits +/// @param /// @return A torque-limited DrivetrainCommand_s static DrivetrainCommand_s TCTorqueLimit( DrivetrainCommand_s command, - float torqueLimits[NUM_MOTORS] -); + float torqueLimits[NUM_MOTORS]); template class TorqueController { -private: +protected: + void TCPowerLimitScaleDown( + DrivetrainCommand_s &command, + const DrivetrainDynamicReport_s &drivetrainData, + float powerLimit) + { + // TODO + // probably requires AMS interface + } + void TCPosTorqueLimit(DrivetrainCommand_s &command, float torqueLimit) + { + for (int i = 0; i < NUM_MOTORS; i++) + { + command.torqueSetpoints[i] = std::min(command.torqueSetpoints[i], torqueLimit); + } + } + public: }; class TorqueControllerNone : public TorqueController { private: - DrivetrainCommand_s data_ = { - .speeds_rpm = {0.0, 0.0, 0.0, 0.0}, - .torqueSetpoints= {0.0, 0.0, 0.0, 0.0} - }; public: - TorqueControllerNone(DrivetrainCommand_s& writeout) + TorqueControllerNone(TorqueControllerOutput_s &writeout) { - writeout = data_; + writeout.command = TC_COMMAND_NO_TORQUE; + writeout.ready = true; }; }; @@ -66,18 +95,90 @@ class TorqueControllerNone : public TorqueController class TorqueControllerSimple : public TorqueController { private: - DrivetrainCommand_s& writeout_; - DrivetrainCommand_s data_; + TorqueControllerOutput_s &writeout_; float frontTorqueScale_ = 1.0; float rearTorqueScale_ = 1.0; + float frontRegenTorqueScale_ = 1.0; + float rearRegenTorqueScale_ = 1.0; + public: - TorqueControllerSimple(DrivetrainCommand_s& writeout, float rearTorqueScale) - : writeout_(writeout), - frontTorqueScale_(2.0 - rearTorqueScale), - rearTorqueScale_(rearTorqueScale) {} - TorqueControllerSimple(DrivetrainCommand_s& writeout) : TorqueControllerSimple(writeout, 1.0) {} + /// @brief simple TC in which a scaling can be applied to both regen and accel torques for scaling accel request (accel percent - regen percent) + /// @param writeout the reference to the torque controller output being sent that contains the drivetrain command + /// @param rearTorqueScale the 0 to 2 scaling with which 0 represents 200 percent of the accel percent with which to request torque from front wheels, 2 being vice versa to the rear and 1 being balanced. + /// @param regenTorqueScale same as rearTorqueScale, accept applied to negative accel percents which correspond to regen + + TorqueControllerSimple(TorqueControllerOutput_s &writeout, float rearTorqueScale, float regenTorqueScale) + : writeout_(writeout), + frontTorqueScale_(2.0 - rearTorqueScale), + rearTorqueScale_(rearTorqueScale), + frontRegenTorqueScale_(2.0 - regenTorqueScale), + rearRegenTorqueScale_(regenTorqueScale) + { + writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.ready = true; + } + TorqueControllerSimple(TorqueControllerOutput_s &writeout) : TorqueControllerSimple(writeout, 1.0, 1.0) {} + + void tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit); +}; + +class TorqueControllerLoadCellVectoring : public TorqueController +{ +private: + TorqueControllerOutput_s &writeout_; + float frontTorqueScale_ = 1.0; + float rearTorqueScale_ = 1.0; + /* + FIR filter designed with + http://t-filter.appspot.com + + sampling frequency: 100 Hz + + * 0 Hz - 10 Hz + gain = 1 + desired ripple = 5 dB + actual ripple = 1.7659949026015025 dB + + * 40 Hz - 50 Hz + gain = 0 + desired attenuation = -40 dB + actual attenuation = -47.34009380570117 dB + */ + const static int numFIRTaps_ = 5; + float FIRTaps_[numFIRTaps_] = { + 0.07022690881526232, + 0.27638313122745306, + 0.408090001549378, + 0.27638313122745306, + 0.07022690881526232}; + int FIRCircBufferHead = 0; // index of the latest sample in the raw buffer + float loadCellForcesRaw_[4][numFIRTaps_] = {}; + float loadCellForcesFiltered_[4] = {}; + // Some checks that can disable the controller + const int errorCountThreshold_ = 25; + int loadCellsErrorCounter_[4] = {}; + bool FIRSaturated_ = false; + bool ready_ = false; + +public: + TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout, float rearTorqueScale) + : writeout_(writeout), + frontTorqueScale_(2.0 - rearTorqueScale), + rearTorqueScale_(rearTorqueScale) + { + writeout_.command = TC_COMMAND_NO_TORQUE; + writeout_.ready = false; + } + TorqueControllerLoadCellVectoring(TorqueControllerOutput_s &writeout) : TorqueControllerLoadCellVectoring(writeout, 1.0) {} - void tick(const SysTick_s& tick, const PedalsSystemData_s& pedalsData, float torqueLimit); + void tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + float torqueLimit, + const AnalogConversion_s &flLoadCellData, + const AnalogConversion_s &frLoadCellData, + const AnalogConversion_s &rlLoadCellData, + const AnalogConversion_s &rrLoadCellData); }; #endif /* __TORQUECONTROLLERS_H__ */ diff --git a/lib/systems/src/Buzzer.cpp b/lib/systems/src/Buzzer.cpp index ace9723cd..f75701751 100644 --- a/lib/systems/src/Buzzer.cpp +++ b/lib/systems/src/Buzzer.cpp @@ -1,5 +1,9 @@ #include "Buzzer.h" #include "Logger.h" +void BuzzerController::deactivate() +{ + buzzer_on_ = false; +} void BuzzerController::activate_buzzer(unsigned long act_time) { last_activation_time_ = act_time; @@ -8,7 +12,7 @@ void BuzzerController::activate_buzzer(unsigned long act_time) bool BuzzerController::done(unsigned long curr_time){ - buzzer_on_ = ((curr_time - last_activation_time_) > buzzer_period_); - return buzzer_on_; + buzzer_on_ = ((curr_time - last_activation_time_) < buzzer_period_); + return (!buzzer_on_); } \ No newline at end of file diff --git a/lib/systems/src/PedalsSystem.cpp b/lib/systems/src/PedalsSystem.cpp index b3b8d20d6..2a1a71905 100644 --- a/lib/systems/src/PedalsSystem.cpp +++ b/lib/systems/src/PedalsSystem.cpp @@ -1,6 +1,10 @@ #include "PedalsSystem.h" #include +#include + +// #include + // TODO parameterize percentages in constructor void PedalsSystem::tick(const SysTick_s &tick, const AnalogConversion_s &accel1, const AnalogConversion_s &accel2, const AnalogConversion_s &brake1, const AnalogConversion_s &brake2) { @@ -13,7 +17,10 @@ PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel const AnalogConversion_s &brake2, unsigned long curr_time) { + PedalsSystemData_s out; + + out.accelImplausible = evaluate_pedal_implausibilities_(accel1, accel2, accelParams_, 0.1); out.brakeImplausible = evaluate_pedal_implausibilities_(brake1, brake2, brakeParams_, 0.25); out.brakeAndAccelPressedImplausibility = evaluate_brake_and_accel_pressed_(accel1, accel2, brake1, brake2); @@ -30,9 +37,11 @@ PedalsSystemData_s PedalsSystem::evaluate_pedals(const AnalogConversion_s &accel out.accelPercent = (accel1.conversion + accel2.conversion) / 2.0; out.brakePercent = (brake1.conversion + brake2.conversion) / 2.0; + out.regenPercent = std::max(std::min(out.brakePercent / mechBrakeActiveThreshold_, 1.0f), 0.0f); out.brakePressed = pedal_is_active_(brake1.conversion, brake2.conversion, brakeParams_.activation_percentage); out.mechBrakeActive = out.brakePercent > mechBrakeActiveThreshold_; out.implausibilityExceededMaxDuration = max_duration_of_implausibility_exceeded_(curr_time); + return out; } @@ -56,26 +65,46 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pe { // FSAE EV.5.5 // FSAE T.4.2.10 - bool pedal_1_less_than_min = (pedalData1.raw < params.min_sense_1); - bool pedal_2_less_than_min = (pedalData2.raw < params.min_sense_2); - bool pedal_1_greater_than_max = (pedalData1.raw > params.max_sense_1); - bool pedal_2_greater_than_max = (pedalData2.raw > params.max_sense_2); + bool pedal1_swapped = false; + bool pedal2_swapped = false; + float margin_of_error = 0.10; + if (params.min_sense_1 > params.max_sense_1) + { + + pedal1_swapped = true; + // swap the logic: need to check and see if it is greater than min and less than max + } + if (params.min_sense_2 > params.max_sense_2) + { + pedal2_swapped = true; + // swap the logic + } + + bool pedal_1_less_than_min = pedal1_swapped ? (pedalData1.raw > ((1.0 + margin_of_error) * params.min_sense_1)) + : (pedalData1.raw < ((1.0 - margin_of_error) * params.min_sense_1)); - // check that the pedals are reading within 10% of each other - // T.4.2.4 + bool pedal_2_less_than_min = pedal2_swapped ? (pedalData2.raw > ((1.0 + margin_of_error) * params.min_sense_2)) + : (pedalData2.raw < ((1.0 - margin_of_error) * params.min_sense_2)); + + bool pedal_1_greater_than_max = pedal1_swapped ? (pedalData1.raw < ((1.0 - margin_of_error) * params.max_sense_1)) + : (pedalData1.raw > ((1.0 + margin_of_error) * params.max_sense_1)); + + bool pedal_2_greater_than_max = pedal2_swapped ? (pedalData2.raw < ((1.0 - margin_of_error) * params.max_sense_2)) + : (pedalData2.raw > ((1.0 + margin_of_error) * params.max_sense_2)); + + bool sens_not_within_req_percent = (fabs(pedalData1.conversion - pedalData2.conversion) > max_percent_diff); - bool pedalsClamped = (pedalData1.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED || pedalData2.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED); + // bool pedalsClamped = (pedalData1.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED || pedalData2.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED); if ( pedal_1_less_than_min || pedal_2_less_than_min || pedal_1_greater_than_max || pedal_2_greater_than_max) { - return true; } - else if (sens_not_within_req_percent || pedalsClamped) + else if (sens_not_within_req_percent) { return true; } @@ -85,15 +114,29 @@ bool PedalsSystem::evaluate_pedal_implausibilities_(const AnalogConversion_s &pe } } +float PedalsSystem::remove_deadzone_(float conversion_input, float deadzone) +{ + // first, subtract the start zone and ensure that the pedal is clamped at zero + float out = std::max(conversion_input - deadzone, 0.0f); + + // second, scale output from 0 to 1 with 1 being 100 percent at say 95 percent actually + out = std::min((out / (1 - deadzone)), 1.0f); + return out; +} + bool PedalsSystem::evaluate_brake_and_accel_pressed_(const AnalogConversion_s &accelPedalData1, const AnalogConversion_s &accelPedalData2, const AnalogConversion_s &brakePedalData1, const AnalogConversion_s &brakePedalData2) { - bool accel_pressed = pedal_is_active_(accelPedalData1.conversion, accelPedalData2.conversion, accelParams_.activation_percentage); // .1 - bool brake_pressed = pedal_is_active_(brakePedalData2.conversion, brakePedalData1.conversion, brakeParams_.activation_percentage); // 0.05 - + float accel_pedal1_real = remove_deadzone_(accelPedalData1.conversion, 0.05); + float accel_pedal2_real = remove_deadzone_(accelPedalData2.conversion, 0.05); + bool accel_pressed = pedal_is_active_(accel_pedal1_real, accel_pedal2_real, accelParams_.activation_percentage); // .1 + bool brake_pressed = pedal_is_active_(brakePedalData2.conversion, brakePedalData1.conversion, 0.80); // 0.05 + // Serial.println("brake percents:"); + // Serial.println(brakePedalData1.conversion); + // Serial.println(brakePedalData2.conversion); return (accel_pressed && brake_pressed); } diff --git a/lib/systems/src/SafetySystem.cpp b/lib/systems/src/SafetySystem.cpp index 77e545831..73256b5e1 100644 --- a/lib/systems/src/SafetySystem.cpp +++ b/lib/systems/src/SafetySystem.cpp @@ -19,12 +19,15 @@ void SafetySystem::software_shutdown(const SysTick_s &tick) { // If AMS heartbeat is not received within reasonable interval // Set software is not ok if (!ams_->heartbeat_received(tick.millis)) { + // Serial.println("heartbeat not received"); software_is_ok = false; } - if (software_is_ok) + if (software_is_ok) { ams_->set_state_ok_high(true); - else + } + else { ams_->set_state_ok_high(false); + } // Kick watchdog every software cycle wd_->kick_watchdog(tick.millis); diff --git a/lib/systems/src/TorqueControllerMux.cpp b/lib/systems/src/TorqueControllerMux.cpp index 5d981abab..3954f0510 100644 --- a/lib/systems/src/TorqueControllerMux.cpp +++ b/lib/systems/src/TorqueControllerMux.cpp @@ -3,30 +3,26 @@ #include "PhysicalParameters.h" void TorqueControllerMux::tick( - const SysTick_s& tick, - const DrivetrainDynamicReport_s& drivetrainData, - const PedalsSystemData_s& pedalsData, - const SteeringSystemData_s& steeringData, - const AnalogConversion_s& loadFLData, - const AnalogConversion_s& loadFRData, - const AnalogConversion_s& loadRLData, - const AnalogConversion_s& loadRRData, - DialMode_e dashboardDialMode, - bool dashboardTorqueModeButtonPressed - ) + const SysTick_s &tick, + const DrivetrainDynamicReport_s &drivetrainData, + const PedalsSystemData_s &pedalsData, + const SteeringSystemData_s &steeringData, + const AnalogConversion_s &loadFLData, + const AnalogConversion_s &loadFRData, + const AnalogConversion_s &loadRLData, + const AnalogConversion_s &loadRRData, + DialMode_e dashboardDialMode, + bool dashboardTorqueModeButtonPressed) { // Tick all torque controllers torqueControllerSimple_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_]); - + torqueControllerLoadCellVectoring_.tick(tick, pedalsData, torqueLimitMap_[torqueLimit_], loadFLData, loadFRData, loadRLData, loadRRData); // Tick torque button logic at 50hz if (tick.triggers.trigger50) { // detect high-to-low transition and lock out button presses for DEBOUNCE_MILLIS ms if ( - torqueLimitButtonPressed_ == true - && dashboardTorqueModeButtonPressed == false - && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS - ) + torqueLimitButtonPressed_ == true && dashboardTorqueModeButtonPressed == false && tick.millis - torqueLimitButtonPressedTime_ > DEBOUNCE_MILLIS) { // WOW C++ is ass torqueLimit_ = static_cast((static_cast(torqueLimit_) + 1) % (static_cast(TorqueLimit_e::TCMUX_NUM_TORQUE_LIMITS))); @@ -42,20 +38,21 @@ void TorqueControllerMux::tick( // Only transisiton to the next state if: // Vehicle speed is below 5 m/s // AND torque delta between old and new controllers is < 0.5nm on every wheel + // AND the selected torque controller is in the ready state if (muxMode_ != dialModeMap_[dashboardDialMode]) { + // Check if speed permits mode change bool speedPreventsModeChange = false; for (int i = 0; i < NUM_MOTORS; i++) speedPreventsModeChange |= drivetrainData.measuredSpeeds[i] * RPM_TO_METERS_PER_SECOND >= MAX_SPEED_FOR_MODE_CHANGE; + // Check if torque delta permits mode change bool torqueDeltaPreventsModeChange = false; for (int i = 0; i < NUM_MOTORS; i++) { float torqueDelta = abs( - controllerCommands_[static_cast(muxMode_)].torqueSetpoints[i] - - controllerCommands_[static_cast(dialModeMap_[dashboardDialMode])].torqueSetpoints[i] - ); - + controllerOutputs_[static_cast(muxMode_)].command.torqueSetpoints[i] - controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].command.torqueSetpoints[i]); + if (torqueDelta > MAX_TORQUE_DELTA_FOR_MODE_CHANGE) { torqueDeltaPreventsModeChange = true; @@ -63,12 +60,23 @@ void TorqueControllerMux::tick( } } - if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange)) + // Check if targeted controller is ready to be selected + bool controllerNotReadyPreventsModeChange = (controllerOutputs_[static_cast(dialModeMap_[dashboardDialMode])].ready == false); + + if (!(speedPreventsModeChange || torqueDeltaPreventsModeChange || controllerNotReadyPreventsModeChange)) { muxMode_ = dialModeMap_[dashboardDialMode]; } } - drivetrainCommand_ = controllerCommands_[static_cast(muxMode_)]; + // Check if the current controller is ready. If it has faulted, revert to safe mode + // When the car goes below 5m/s, it will attempt to re-engage the faulted controller + // It will stay in safe mode if the controller is still faulted + if (controllerOutputs_[static_cast(muxMode_)].ready == false) + { + muxMode_ = TorqueController_e::TC_SAFE_MODE; + } + + drivetrainCommand_ = controllerOutputs_[static_cast(muxMode_)].command; } } \ No newline at end of file diff --git a/lib/systems/src/TorqueControllers.cpp b/lib/systems/src/TorqueControllers.cpp index 96e845165..01ec3b88d 100644 --- a/lib/systems/src/TorqueControllers.cpp +++ b/lib/systems/src/TorqueControllers.cpp @@ -1,89 +1,157 @@ #include "TorqueControllers.h" #include "Utility.h" #include +#include "PhysicalParameters.h" -static inline void TCPowerLimitScaleDown( - DrivetrainCommand_s &command, - const DrivetrainDynamicReport_s &drivetrainData, - float powerLimit -) -{ - // TODO - // probably requires AMS interface -} +// TorqueControllerSimple -static inline void TCPosTorqueLimit(DrivetrainCommand_s &command, float torqueLimit) +void TorqueControllerSimple::tick(const SysTick_s &tick, const PedalsSystemData_s &pedalsData, float torqueLimit) { - for (int i = 0; i < NUM_MOTORS; i++) + + // Calculate torque commands at 100hz + if (tick.triggers.trigger100) { - command.torqueSetpoints[i] = std::min(command.torqueSetpoints[i], torqueLimit); + // Both pedals are not pressed and no implausibility has been detected + // accelRequest goes between 1.0 and -1.0 + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torqueRequest; + + if (accelRequest >= 0.0) + { + // Positive torque request + torqueRequest = accelRequest * AMK_MAX_TORQUE; + + // writeout_.command.speeds_rpm[FL] = accelRequest * AMK_MAX_RPM; + // writeout_.command.speeds_rpm[FR] = accelRequest * AMK_MAX_RPM; + // writeout_.command.speeds_rpm[RL] = accelRequest * AMK_MAX_RPM; + // writeout_.command.speeds_rpm[RR] = accelRequest * AMK_MAX_RPM; + writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; + + writeout_.command.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; + writeout_.command.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; + writeout_.command.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; + writeout_.command.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; + } + else + { + // Negative torque request + torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + writeout_.command.speeds_rpm[RL] = 0.0; + writeout_.command.speeds_rpm[RR] = 0.0; + + writeout_.command.torqueSetpoints[FL] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[FR] = torqueRequest * frontRegenTorqueScale_; + writeout_.command.torqueSetpoints[RL] = torqueRequest * rearRegenTorqueScale_; + writeout_.command.torqueSetpoints[RR] = torqueRequest * rearRegenTorqueScale_; + } + + // Apply the torque limit + TCPosTorqueLimit(writeout_.command, torqueLimit); } } -// TorqueControllerSimple +// TorqueControllerLoadCellVectoring -void TorqueControllerSimple::tick(const SysTick_s& tick, const PedalsSystemData_s& pedalsData, float torqueLimit) +void TorqueControllerLoadCellVectoring::tick( + const SysTick_s &tick, + const PedalsSystemData_s &pedalsData, + float torqueLimit, + const AnalogConversion_s &flLoadCellData, + const AnalogConversion_s &frLoadCellData, + const AnalogConversion_s &rlLoadCellData, + const AnalogConversion_s &rrLoadCellData) { // Calculate torque commands at 100hz if (tick.triggers.trigger100) { - if ((!pedalsData.brakeAndAccelPressedImplausibility) && (pedalsData.implausibilityExceededMaxDuration== false)) + // Apply FIR filter to load cell data + loadCellForcesRaw_[0][FIRCircBufferHead] = flLoadCellData.conversion; + loadCellForcesRaw_[1][FIRCircBufferHead] = frLoadCellData.conversion; + loadCellForcesRaw_[2][FIRCircBufferHead] = rlLoadCellData.conversion; + loadCellForcesRaw_[3][FIRCircBufferHead] = rrLoadCellData.conversion; + + for (int i = 0; i < 4; i++) + { + loadCellForcesFiltered_[i] = 0.0f; + for (int FIROffset = 0; FIROffset < numFIRTaps_; FIROffset++) + { + int index = (FIRCircBufferHead + FIROffset) % numFIRTaps_; + loadCellForcesFiltered_[i] += loadCellForcesRaw_[i][index] * FIRTaps_[FIROffset]; + } + } + FIRCircBufferHead = (FIRCircBufferHead + 1) % numFIRTaps_; + if (FIRCircBufferHead == numFIRTaps_ - 1) + FIRSaturated_ = true; + + // Do sanity checks on raw data + loadCellsErrorCounter_[0] = flLoadCellData.raw != 4095 && flLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[0] + 1; + loadCellsErrorCounter_[1] = frLoadCellData.raw != 4095 && frLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[1] + 1; + loadCellsErrorCounter_[2] = rlLoadCellData.raw != 4095 && rlLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[2] + 1; + loadCellsErrorCounter_[3] = rrLoadCellData.raw != 4095 && rrLoadCellData.status != AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED ? 0 : loadCellsErrorCounter_[3] + 1; + ready_ = FIRSaturated_ && loadCellsErrorCounter_[0] < errorCountThreshold_ && loadCellsErrorCounter_[1] < errorCountThreshold_ && loadCellsErrorCounter_[2] < errorCountThreshold_ && loadCellsErrorCounter_[3] < errorCountThreshold_; + + writeout_.ready = ready_; + + if (ready_) { + // Calculate total normal force + float sumNormalForce = 0.0f; + for (int i = 0; i < 4; i++) + { + sumNormalForce += loadCellForcesFiltered_[i]; + } + // Both pedals are not pressed and no implausibility has been detected // accelRequest goes between 1.0 and -1.0 - float accelRequest = pedalsData.accelPercent - pedalsData.brakePercent; + float accelRequest = pedalsData.accelPercent - pedalsData.regenPercent; + float torquePool; float torqueRequest; if (accelRequest >= 0.0) { // Positive torque request - torqueRequest = accelRequest * AMK_MAX_TORQUE; + // NOTE: using "torquePool" here instead of torqueRequest for legibility + torquePool = accelRequest * AMK_MAX_TORQUE * 4; - data_.speeds_rpm[FL] = AMK_MAX_RPM; - data_.speeds_rpm[FR] = AMK_MAX_RPM; - data_.speeds_rpm[RL] = AMK_MAX_RPM; - data_.speeds_rpm[RR] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[FL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[FR] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RL] = AMK_MAX_RPM; + writeout_.command.speeds_rpm[RR] = AMK_MAX_RPM; - data_.torqueSetpoints[FL] = torqueRequest * frontTorqueScale_; - data_.torqueSetpoints[FR] = torqueRequest * frontTorqueScale_; - data_.torqueSetpoints[RL] = torqueRequest * rearTorqueScale_; - data_.torqueSetpoints[RR] = torqueRequest * rearTorqueScale_; + writeout_.command.torqueSetpoints[FL] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[0] / sumNormalForce; + writeout_.command.torqueSetpoints[FR] = torquePool * frontTorqueScale_ * loadCellForcesFiltered_[1] / sumNormalForce; + writeout_.command.torqueSetpoints[RL] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[2] / sumNormalForce; + writeout_.command.torqueSetpoints[RR] = torquePool * rearTorqueScale_ * loadCellForcesFiltered_[3] / sumNormalForce; } else { // Negative torque request - torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; - - data_.speeds_rpm[FL] = 0.0; - data_.speeds_rpm[FR] = 0.0; - data_.speeds_rpm[RL] = 0.0; - data_.speeds_rpm[RR] = 0.0; - - data_.torqueSetpoints[FL] = torqueRequest; - data_.torqueSetpoints[FR] = torqueRequest; - data_.torqueSetpoints[RL] = torqueRequest; - data_.torqueSetpoints[RR] = torqueRequest; + // No load cell vectoring on regen + torqueRequest = MAX_REGEN_TORQUE * accelRequest * -1.0; + + writeout_.command.speeds_rpm[FL] = 0.0; + writeout_.command.speeds_rpm[FR] = 0.0; + writeout_.command.speeds_rpm[RL] = 0.0; + writeout_.command.speeds_rpm[RR] = 0.0; + + writeout_.command.torqueSetpoints[FL] = torqueRequest; + writeout_.command.torqueSetpoints[FR] = torqueRequest; + writeout_.command.torqueSetpoints[RL] = torqueRequest; + writeout_.command.torqueSetpoints[RR] = torqueRequest; } // Apply the torque limit - TCPosTorqueLimit(data_, torqueLimit); + TCPosTorqueLimit(writeout_.command, torqueLimit); } else { - // Both pedals are pressed or an implausibility has been detected - // Zero out torque - data_.speeds_rpm[FL] = 0.0; - data_.speeds_rpm[FR] = 0.0; - data_.speeds_rpm[RL] = 0.0; - data_.speeds_rpm[RR] = 0.0; - - data_.torqueSetpoints[FL] = 0.0; - data_.torqueSetpoints[FR] = 0.0; - data_.torqueSetpoints[RL] = 0.0; - data_.torqueSetpoints[RR] = 0.0; - + writeout_.command = TC_COMMAND_NO_TORQUE; } - - writeout_ = data_; } } \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 735f8f4a4..0718e6ac9 100644 --- a/platformio.ini +++ b/platformio.ini @@ -12,6 +12,7 @@ test_ignore= test_interfaces* lib_deps= https://github.com/hytech-racing/shared_firmware_systems.git + [env:teensy41] ; Testing variables test_framework=unity @@ -46,9 +47,10 @@ lib_deps = https://github.com/hytech-racing/shared_firmware_systems.git https://github.com/RCMast3r/spi_libs https://github.com/tonton81/FlexCAN_T4 - https://github.com/RCMast3r/hytech_can - https://github.com/hytech-racing/HT_CAN/releases/latest/download/can_lib.tar.gz - + https://github.com/RCMast3r/hytech_can#testing_new_inv_ids + ; https://github.com/hytech-racing/HT_CAN/releases/latest/download/can_lib.tar.gz + https://github.com/hytech-racing/HT_CAN/releases/download/29/can_lib.tar.gz + [env:test_can_on_teensy] lib_ignore = mock_interfaces diff --git a/src/main.cpp b/src/main.cpp index 671b1f650..bc21082a8 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,6 +17,7 @@ #include "DashboardInterface.h" #include "InverterInterface.h" #include "TelemetryInterface.h" +#include "SABInterface.h" /* Systems */ #include "SysClock.h" @@ -26,7 +27,7 @@ #include "PedalsSystem.h" #include "TorqueControllerMux.h" -/* State machine */ +// /* State machine */ #include "MCUStateMachine.h" /* @@ -41,28 +42,31 @@ FlexCAN_T4 TELEM_CAN; // telemetry CAN (basically using CircularBufferType = Circular_Buffer; /* Sensors */ -struct ADCs -{ - MCP_ADC<8> a1 = MCP_ADC<8>(ADC1_CS); - MCP_ADC<4> a2 = MCP_ADC<4>(ADC2_CS); - MCP_ADC<4> a3 = MCP_ADC<4>(ADC3_CS); -} ADC; +MCP_ADC<8> a1 = MCP_ADC<8>(ADC1_CS); +MCP_ADC<4> a2 = MCP_ADC<4>(ADC2_CS, 1000000); // 1M baud needed for 04s +MCP_ADC<4> a3 = MCP_ADC<4>(ADC3_CS, 1000000); -OrbisBR10 steering1(STEERING_SERIAL); +OrbisBR10 steering1(&Serial5); -/* - INTERFACES -*/ +// /* +// INTERFACES +// */ -DashboardInterface dashboard(&CAN2_txBuffer); -AMSInterface ams_interface(SOFTWARE_OK); -WatchdogInterface wd_interface(WATCHDOG_INPUT); +DashboardInterface dashboard(&CAN3_txBuffer); +AMSInterface ams_interface(8); +WatchdogInterface wd_interface(32); MCUInterface main_ecu(&CAN3_txBuffer); TelemetryInterface telem_interface(&CAN3_txBuffer, {MCU15_ACCEL1_CHANNEL, MCU15_ACCEL2_CHANNEL, MCU15_BRAKE1_CHANNEL, MCU15_BRAKE2_CHANNEL, MCU15_FL_POTS_CHANNEL, MCU15_FR_POTS_CHANNEL, MCU15_FL_LOADCELL_CHANNEL, MCU15_FR_LOADCELL_CHANNEL, MCU15_STEERING_CHANNEL, MCU15_CUR_POS_SENSE_CHANNEL, MCU15_CUR_NEG_SENSE_CHANNEL, MCU15_GLV_SENSE_CHANNEL}); - -/* Inverter Interface Type */ +SABInterface sab_interface( + LOADCELL_RL_SCALE, // RL Scale + LOADCELL_RL_OFFSET, // RL Offset (Migos) + LOADCELL_RR_SCALE, // RR Scale + LOADCELL_RR_OFFSET // RR Offset +); + +// /* Inverter Interface Type */ using InvInt_t = InverterInterface; struct inverters { @@ -72,29 +76,31 @@ struct inverters InvInt_t rr = InvInt_t(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND); } inv; -/* - SYSTEMS -*/ +// /* +// SYSTEMS +// */ SysClock sys_clock; SteeringSystem steering_system(&steering1); BuzzerController buzzer(BUZZER_ON_INTERVAL); + SafetySystem safety_system(&ams_interface, &wd_interface); +// SafetySystem safety_system(&ams_interface, &wd_interface, &dashboard); PedalsSystem pedals_system({ACCEL1_MIN_THRESH, ACCEL2_MIN_THRESH, ACCEL1_MAX_THRESH, ACCEL2_MAX_THRESH, APPS_ACTIVATION_PERCENTAGE}, - {BRAKE1_MIN_THRESH, BRAKE2_MIN_THRESH, BRAKE1_MAX_THRESH, BRAKE2_MAX_THRESH, BRAKE_ACTIVATION_PERCENTAGE}, + {BRAKE1_MIN_THRESH, BRAKE2_MIN_THRESH, BRAKE1_MAX_THRESH, BRAKE2_MAX_THRESH, BRKAE_ACTIVATION_PERCENTAGE}, BRAKE_MECH_THRESH); using DriveSys_t = DrivetrainSystem; DriveSys_t drivetrain = DriveSys_t({&inv.fl, &inv.fr, &inv.rl, &inv.rr}, &main_ecu, INVERTER_ENABLING_TIMEOUT_INTERVAL); -TorqueControllerMux torque_controller_mux; +TorqueControllerMux torque_controller_mux(1.0, 0.4); /* Declare state machine */ MCUStateMachine fsm(&buzzer, &drivetrain, &dashboard, &pedals_system, &torque_controller_mux, &safety_system); -/* - GROUPING STRUCTS (To limit parameter count in utilizing functions) -*/ +// /* +// GROUPING STRUCTS (To limit parameter count in utilizing functions) +// */ -CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &dashboard, &ams_interface}; +CANInterfaces CAN_receive_interfaces = {&inv.fl, &inv.fr, &inv.rl, &inv.rr, &dashboard, &ams_interface, &sab_interface}; /* FUNCTION DEFINITIONS @@ -117,7 +123,29 @@ void setup() { // initialize CAN communication init_all_CAN_devices(); - + + SPI.begin(); + a1.init(); + a2.init(); + a3.init(); + + a1.setChannelScale(MCU15_ACCEL1_CHANNEL, (1.0 / (float)(ACCEL1_MAX_THRESH - ACCEL1_MIN_THRESH))); + a1.setChannelScale(MCU15_ACCEL2_CHANNEL, (1.0 / (float)(ACCEL2_MAX_THRESH - ACCEL2_MIN_THRESH))); + a1.setChannelScale(MCU15_BRAKE1_CHANNEL, (1.0 / (float)(BRAKE1_MAX_THRESH - BRAKE1_MIN_THRESH))); + a1.setChannelScale(MCU15_BRAKE2_CHANNEL, (1.0 / (float)(BRAKE2_MAX_THRESH - BRAKE2_MIN_THRESH))); + a1.setChannelOffset(MCU15_ACCEL1_CHANNEL, -ACCEL1_MIN_THRESH); + a1.setChannelOffset(MCU15_ACCEL2_CHANNEL, -ACCEL2_MIN_THRESH); + a1.setChannelOffset(MCU15_BRAKE1_CHANNEL, -BRAKE1_MIN_THRESH); + a1.setChannelOffset(MCU15_BRAKE2_CHANNEL, -BRAKE2_MIN_THRESH); + + a3.setChannelScale(MCU15_FL_LOADCELL_CHANNEL,LOADCELL_FL_SCALE/*Todo*/); + a3.setChannelScale(MCU15_FR_LOADCELL_CHANNEL,LOADCELL_FR_SCALE/*Todo*/); + + a3.setChannelOffset(MCU15_FL_LOADCELL_CHANNEL,LOADCELL_FL_OFFSET/*Todo*/); + a3.setChannelOffset(MCU15_FR_LOADCELL_CHANNEL,LOADCELL_FR_OFFSET/*Todo*/); + + Serial.begin(115200); + // get latest tick from sys clock SysTick_s curr_tick = sys_clock.tick(micros()); @@ -128,6 +156,7 @@ void setup() main_ecu.init(); // initial shutdown circuit readings, wd_interface.init(curr_tick.millis); // initialize wd kick time ams_interface.init(curr_tick.millis); // initialize last heartbeat time + steering1.init(); /* Init Systems @@ -135,16 +164,13 @@ void setup() safety_system.init(); - // present action for 1 second - delay(SETUP_PRESENT_ACTION_INTERVAL); - // Drivetrain set all inverters disabled - drivetrain.disable(); // write inv_en and inv_24V_en low: writing high in previous code though, should double check - // would an error list be good for debugging? i.e. which inverter has error + drivetrain.disable(); // write inv_en and inv_24V_en low: writing high in previous code though, should double check + // would an error list be good for debugging? i.e. which inverter has error // ControllerMux set max torque to 20 NM, torque mode to whatever makes most sense - // Preventing drivers from forgetting to toggle torque mode and end up self-derating at comp - // Not strictly necessary at the moment, just don't forget + // Preventing drivers from forgetting to toggle torque mode and end up self-derating at comp + // Not strictly necessary at the moment, just don't forget } void loop() @@ -162,17 +188,20 @@ void loop() // tick systems tick_all_systems(curr_tick); - // inverter procedure before entering state machine - // reset inverters - drivetrain_reset(); - + // // inverter procedure before entering state machine + // // reset inverters + if (dashboard.inverterResetButtonPressed() && drivetrain.drivetrain_error_occured()) + { + hal_println("resetting errored drivetrain"); + drivetrain.reset_drivetrain(); + } // tick state machine fsm.tick_state_machine(curr_tick.millis); // tick safety system safety_system.software_shutdown(curr_tick); - // send CAN + // send CAN send_all_CAN_msgs(CAN2_txBuffer, &INV_CAN); send_all_CAN_msgs(CAN3_txBuffer, &TELEM_CAN); } @@ -210,85 +239,83 @@ void tick_all_interfaces(const SysTick_s ¤t_system_tick) { TriggerBits_s t = current_system_tick.triggers; - if (t.trigger10) // 10Hz { - dashboard.soundBuzzer(buzzer.buzzer_is_on()); - dashboard.write(); + // Serial.println("before buzzer"); + dashboard.tick10(&main_ecu, int(fsm.get_state()), + buzzer.buzzer_is_on(), + drivetrain.drivetrain_error_occured(), + torque_controller_mux.getTorqueLimit()); main_ecu.tick(static_cast(fsm.get_state()), - drivetrain.drivetrain_error_occured(), - safety_system.get_software_is_ok(), - static_cast(torque_controller_mux.getTorqueLimit()), - torque_controller_mux.getMaxTorque(), - buzzer.buzzer_is_on(), - pedals_system.getPedalsSystemData(), - ams_interface.pack_charge_is_critical(), - dashboard.launchControlButtonPressed()); + drivetrain.drivetrain_error_occured(), + safety_system.get_software_is_ok(), + static_cast(torque_controller_mux.getTorqueLimit()), + torque_controller_mux.getMaxTorque(), + buzzer.buzzer_is_on(), + pedals_system.getPedalsSystemData(), + ams_interface.pack_charge_is_critical(), + dashboard.launchControlButtonPressed()); } if (t.trigger50) // 50Hz { - telem_interface.tick(ADC.a1.get(), ADC.a2.get(), ADC.a3.get(), steering1.convert()); + steering1.sample(); + PedalsSystemData_s data2 = pedals_system.getPedalsSystemDataCopy(); + telem_interface.tick(a1.get(), a2.get(), a3.get(), steering1.convert(), &inv.fl, &inv.fr, &inv.rl, &inv.rr, data2.accelImplausible, data2.brakeImplausible, data2.accelPercent, data2.brakePercent); } if (t.trigger100) // 100Hz { - ADC.a1.tick(); - ADC.a2.tick(); - ADC.a3.tick(); - } - // Untriggered + a1.tick(); + a2.tick(); + a3.tick(); + } + // // Untriggered main_ecu.read_mcu_status(); // should be executed at the same rate as state machine // DO NOT call in main_ecu.tick() - } -/* - TICK SYSTEMS -*/ +// /* +// TICK SYSTEMS +// */ void tick_all_systems(const SysTick_s ¤t_system_tick) { // tick pedals system - pedals_system.tick( - current_system_tick, - ADC.a1.get().conversions[MCU15_ACCEL1_CHANNEL], - ADC.a1.get().conversions[MCU15_ACCEL2_CHANNEL], - ADC.a1.get().conversions[MCU15_BRAKE1_CHANNEL], - ADC.a1.get().conversions[MCU15_BRAKE2_CHANNEL]); - // tick steering system - steering_system.tick( - current_system_tick, - ADC.a1.get().conversions[MCU15_STEERING_CHANNEL]); + // Serial.println("accel1"); + // Serial.println(a1.get().conversions[MCU15_ACCEL1_CHANNEL].raw); + // Serial.println("accel2"); + // Serial.println(a1.get().conversions[MCU15_ACCEL2_CHANNEL].raw); + // Serial.println("brake1"); + // Serial.println(a1.get().conversions[MCU15_BRAKE1_CHANNEL].raw); + // Serial.println("brake2"); + // Serial.println(a1.get().conversions[MCU15_BRAKE2_CHANNEL].raw); - // tick drivetrain system + pedals_system.tick( + current_system_tick, + a1.get().conversions[MCU15_ACCEL1_CHANNEL], + a1.get().conversions[MCU15_ACCEL2_CHANNEL], + a1.get().conversions[MCU15_BRAKE1_CHANNEL], + a1.get().conversions[MCU15_BRAKE2_CHANNEL]); + // // tick steering system + // steering_system.tick( + // current_system_tick, + // a1.get().conversions[MCU15_STEERING_CHANNEL]); + + // // tick drivetrain system drivetrain.tick(current_system_tick); - - // tick torque controller mux + // // tick torque controller mux torque_controller_mux.tick( current_system_tick, drivetrain.get_current_data(), pedals_system.getPedalsSystemData(), steering_system.getSteeringSystemData(), - ADC.a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], // FL load cell reading. TODO: fix index - ADC.a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], // FR load cell reading. TODO: fix index - (const AnalogConversion_s){}, // RL load cell reading. TODO: get data from rear load cells - (const AnalogConversion_s){}, // RR load cell reading. TODO: get data from rear load cells + a2.get().conversions[MCU15_FL_LOADCELL_CHANNEL], // FL load cell reading. TODO: fix index + a3.get().conversions[MCU15_FR_LOADCELL_CHANNEL], // FR load cell reading. TODO: fix index + sab_interface.rlLoadCell.convert(), // RL load cell reading. TODO: get data from rear load cells + sab_interface.rrLoadCell.convert(), // RR load cell reading. TODO: get data from rear load cells dashboard.getDialMode(), - dashboard.torqueButtonPressed()); -} - -/* - Finish restarting when timer expires -*/ -void drivetrain_reset() -{ - if (dashboard.inverterResetButtonPressed()) - { - drivetrain.enable_drivetrain_reset(); - } - drivetrain.check_reset_condition(); - drivetrain.reset_drivetrain(); + dashboard.torqueModeButtonPressed()); } diff --git a/src/main_spi_test.cpp b/src/main_spi_test.cpp new file mode 100644 index 000000000..c814f46bb --- /dev/null +++ b/src/main_spi_test.cpp @@ -0,0 +1,25 @@ +#include +#include "MCP_ADC.h" +const int SS_PIN = 34; // assuming MOSI 11, MISO 12, SCK 13 + + +MCP_ADC<8> a1 = MCP_ADC<8>(34); +MCP_ADC<4> a2 = MCP_ADC<4>(33); +MCP_ADC<4> a3 = MCP_ADC<4>(29); + + +void setup() +{ + SPI.begin(); + a1.init(); + a2.init(); + a3.init(); +} + +void loop() +{ + Serial.println("looped"); + a1.tick(); + a2.tick(); + a3.tick(); +} diff --git a/src/old_main.cpp b/src/old_main.cpp deleted file mode 100644 index 8f74d7f3b..000000000 --- a/src/old_main.cpp +++ /dev/null @@ -1,1576 +0,0 @@ - -/* - Teensy 4.1 Main Control Unit code - Written by Liwei Sun which is why the code is so bad - - Rev 12 -*/ -#include "Logger.h" - -#include -#include -#include -#include -#define _USE_MATH_DEFINES -#include -#include -#include - -#include "ADC_SPI.h" -#include "STEERING_SPI.h" -#include "kinetis_flexcan.h" -#include "Metro.h" - -// IMU -#include - -#include "drivers.h" - -// constants to define for different operation - -#define DRIVER DEFAULT_DRIVER -#define TORQUE_1 10 -#define TORQUE_2 15 -#define TORQUE_3 21 -#define MAX_ALLOWED_SPEED 20000 - -// set to true or false for debugging -#define DEBUG false -#define BMS_DEBUG_ENABLE false - -#include "MCU_rev12_dfs.h" - -#include "driver_constants.h" - -// Call the ADIS16460 IMU class -ADIS16460 IMU(IMU_CS, IMU_DATAREADY, IMU_RESET); // Chip Select, Data Ready, Reset Pin Assignments - -// Outbound CAN message/* */s -MCU_pedal_readings mcu_pedal_readings; -MCU_status mcu_status{}; -MCU_load_cells mcu_load_cells{}; -MCU_front_potentiometers mcu_front_potentiometers; -MCU_rear_potentiometers mcu_rear_potentiometers; -MCU_analog_readings mcu_analog_readings{}; - -// IMU -IMU_accelerometer imu_accelerometer; -IMU_gyroscope imu_gyroscope; -double imu_velocity; -double pitch_calibration_angle; - -MC_status mc_status[4]; -MC_temps mc_temps[4]; -MC_energy mc_energy[4]; -MC_setpoints_command mc_setpoints_command[4]; - -// Inbound CAN messages -BMS_coulomb_counts bms_coulomb_counts{}; -BMS_status bms_status{}; -BMS_temperatures bms_temperatures{}; -BMS_voltages bms_voltages{}; -Dashboard_status dashboard_status{}; - -//Timers -Metro timer_imu_integration = Metro(50); -Metro timer_CAN_imu_accelerometer_send = Metro(50); -Metro timer_CAN_imu_gyroscope_send = Metro(50); -Metro timer_CAN_inverter_setpoints_send = Metro(20); -Metro timer_CAN_coloumb_count_send = Metro(1000); -Metro timer_CAN_mcu_status_send = Metro(100); -Metro timer_CAN_mcu_pedal_readings_send = Metro(50); -Metro timer_CAN_mcu_analog_readings_send = Metro(50); -Metro timer_CAN_mcu_load_cells_send = Metro(20); -Metro timer_CAN_mcu_potentiometers_send = Metro(20); - -Metro timer_ready_sound = Metro(2000); // Time to play RTD sound - -Metro timer_read_all_adcs = Metro(20); -Metro timer_steering_spi_read = Metro(1000); -Metro timer_read_imu = Metro(20); - -Metro timer_inverter_enable = Metro(5000); -Metro timer_reset_inverter = Metro(5000); -Metro timer_watchdog_timer = Metro(7); - -elapsedMillis pedal_implausability_duration; - -Metro timer_debug = Metro(200); -Metro timer_debug2 = Metro(1000); - -// this abuses Metro timer functionality to stay faulting once a fault has occurred -// autoreset makes the timer update to the current time and not by the interval -// this allows me to set the interval as 0 once a fault has occurred, leading to continuous faulting -// until a CAN message comes in which resets the timer and the interval -Metro timer_bms_heartbeat = Metro(0, 1); - - -bool imd_faulting = false; -bool inverter_restart = false; // True when restarting the inverter -INVERTER_STARTUP_STATE inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; - -ADC_SPI ADC1(ADC1_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); -ADC_SPI ADC2(ADC2_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); -ADC_SPI ADC3(ADC3_CS, ADC_SPI_SPEED, ECU_SDI, ECU_SDO, ECU_CLK); - -STEERING_SPI STEERING(STEERING_CS, STEERING_SPI_SPEED); - -FlexCAN_T4 FRONT_INV_CAN; -FlexCAN_T4 REAR_INV_CAN; -FlexCAN_T4 TELEM_CAN; -CAN_message_t msg; - -// coloumb counts -uint32_t total_discharge; -unsigned long previous_data_time; - -int16_t torque_setpoint_array[4]; -int16_t speed_setpoint_array[4] = {0, 0, 0, 0}; - -uint16_t prev_load_cell_readings[4] = {0, 0, 0, 0}; -float load_cell_alpha = 0.95; - -float filtered_min_cell_voltage = 3.5; -float cell_voltage_alpha = 0.8; - -float filtered_max_cell_temp = 40.0; -float cell_temp_alpha = 0.8; - -uint16_t current_read = 0; -uint16_t reference_read = 0; - -float max_front_power = 0.0; -float max_rear_power = 0.0; - -enum launch_states {launch_not_ready, launch_ready, launching}; -launch_states launch_state = launch_not_ready; -int16_t launch_speed_target = 0; -elapsedMillis time_since_launch; -const uint16_t LAUNCH_READY_ACCEL_THRESHOLD = 100; -const uint16_t LAUNCH_READY_BRAKE_THRESHOLD = 300; -const int16_t LAUNCH_READY_SPEED_THRESHOLD = 500; -const uint16_t LAUNCH_GO_THRESHOLD = 900; -const uint16_t LAUNCH_STOP_THRESHOLD = 600; -float launch_rate_target = 0.0; - -inline void set_all_inverters_dc_on(bool input); -inline void calculate_pedal_implausibilities(); - -inline float max_allowed_torque(float maxwatts, float rpm); -inline void set_inverter_torques_regen_only(); -inline void set_all_inverters_disabled(); -inline void set_all_inverters_no_torque(); -bool check_all_inverters_quit_dc_on(); -inline void check_TS_active(); -bool check_all_inverters_system_ready(); - -inline void set_inverter_torques(); -uint8_t check_all_inverters_error(); -bool check_all_inverters_quit_inverter_on(); -inline void set_all_inverters_inverter_enable(bool input); -inline void set_all_inverters_no_torque(); - -inline void set_all_inverters_driver_enable(bool input); - -bool check_TS_over_HV_threshold(); - - -void set_state(MCU_STATE new_state); - -inline void send_CAN_inverter_setpoints() { - if (timer_CAN_inverter_setpoints_send.check()) { - mc_setpoints_command[0].write(msg.buf); - msg.id = ID_MC1_SETPOINTS_COMMAND; - msg.len = sizeof(mc_setpoints_command[0]); - FRONT_INV_CAN.write(msg); - - mc_setpoints_command[1].write(msg.buf); - msg.id = ID_MC2_SETPOINTS_COMMAND; - msg.len = sizeof(mc_setpoints_command[1]); - FRONT_INV_CAN.write(msg); - - mc_setpoints_command[2].write(msg.buf); - msg.id = ID_MC3_SETPOINTS_COMMAND; - msg.len = sizeof(mc_setpoints_command[2]); - REAR_INV_CAN.write(msg); - - mc_setpoints_command[3].write(msg.buf); - msg.id = ID_MC4_SETPOINTS_COMMAND; - msg.len = sizeof(mc_setpoints_command[3]); - REAR_INV_CAN.write(msg); - } -} - -inline void send_CAN_mcu_status() { - if (timer_CAN_mcu_status_send.check()) { - // Send Main Control Unit status message - mcu_status.write(msg.buf); - msg.id = ID_MCU_STATUS; - msg.len = sizeof(mcu_status); - TELEM_CAN.write(msg); - } -} - -inline void send_CAN_mcu_pedal_readings() { - if (timer_CAN_mcu_pedal_readings_send.check()) { - mcu_pedal_readings.write(msg.buf); - msg.id = ID_MCU_PEDAL_READINGS; - msg.len = sizeof(mcu_pedal_readings); - TELEM_CAN.write(msg); - } -} - -inline void send_CAN_mcu_load_cells() { - if (timer_CAN_mcu_load_cells_send.check()) { - mcu_load_cells.write(msg.buf); - msg.id = ID_MCU_LOAD_CELLS; - msg.len = sizeof(mcu_load_cells); - TELEM_CAN.write(msg); - } -} - -inline void send_CAN_mcu_potentiometers() { - if (timer_CAN_mcu_potentiometers_send.check()) { - mcu_front_potentiometers.write(msg.buf); - msg.id = ID_MCU_FRONT_POTS; - msg.len = sizeof(mcu_front_potentiometers); - TELEM_CAN.write(msg); - mcu_rear_potentiometers.write(msg.buf); - msg.id = ID_MCU_REAR_POTS; - msg.len = sizeof(mcu_rear_potentiometers); - TELEM_CAN.write(msg); - } -} - -inline void send_CAN_bms_coulomb_counts() { - if (timer_CAN_coloumb_count_send.check()) { - bms_coulomb_counts.write(msg.buf); - msg.id = ID_BMS_COULOMB_COUNTS; - msg.len = sizeof(bms_coulomb_counts); - TELEM_CAN.write(msg); - } -} - -inline void send_CAN_mcu_analog_readings() { - if (timer_CAN_mcu_analog_readings_send.check()) { - mcu_analog_readings.write(msg.buf); - msg.id = ID_MCU_ANALOG_READINGS; - msg.len = sizeof(mcu_analog_readings); - TELEM_CAN.write(msg); - } -} -inline void state_machine() { - switch (mcu_status.get_state()) { - case MCU_STATE::STARTUP: break; - - case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: -#if DEBUG - Serial.println("TS NOT ACTIVE"); -#endif - // if TS is above HV threshold, move to Tractive System Active - if (check_TS_over_HV_threshold()) { -#if DEBUG - Serial.println("Setting state to TS Active from TS Not Active"); -#endif - set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); - } - break; - - case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: - check_TS_active(); - if (check_all_inverters_system_ready()) { - set_all_inverters_dc_on(true); - inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON; - } - if (dashboard_status.get_start_btn() && mcu_status.get_mech_brake_active()) { -#if DEBUG - Serial.println("Setting state to Enabling Inverter"); -#endif - set_state(MCU_STATE::ENABLING_INVERTER); - } - break; - - case MCU_STATE::ENABLING_INVERTER: - check_TS_active(); - // inverter enabling timed out - if (timer_inverter_enable.check()) { -#if DEBUG - Serial.println("Setting state to TS Active from Enabling Inverter"); -#endif - set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); - } - - switch (inverter_startup_state) { - case INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY: - if (check_all_inverters_system_ready()) { - set_all_inverters_dc_on(true); - inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON; - } - break; - - case INVERTER_STARTUP_STATE::WAIT_QUIT_DC_ON: - if (check_all_inverters_quit_dc_on()) { - set_all_inverters_no_torque(); - set_all_inverters_driver_enable(true); - set_all_inverters_inverter_enable(true); - inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_QUIT_INVERTER_ON; - } - break; - - case INVERTER_STARTUP_STATE::WAIT_QUIT_INVERTER_ON: - if (check_all_inverters_quit_inverter_on()) { -#if DEBUG - Serial.println("Setting state to Waiting Ready to Drive Sound"); -#endif - set_state(MCU_STATE::WAITING_READY_TO_DRIVE_SOUND); - } - break; - - } - break; - - - case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: - check_TS_active(); - - // if the ready to drive sound has been playing for long enough, move to ready to drive mode - if (timer_ready_sound.check()) { -#if DEBUG - Serial.println("Setting state to Ready to Drive"); -#endif - set_state(MCU_STATE::READY_TO_DRIVE); - } - break; - - case MCU_STATE::READY_TO_DRIVE: - check_TS_active(); - - if (check_all_inverters_error()) { - set_all_inverters_disabled(); - set_state(MCU_STATE::TRACTIVE_SYSTEM_ACTIVE); - } - - calculate_pedal_implausibilities(); - - if ( - pedal_implausability_duration <= 100 && - mcu_status.get_bms_ok_high() && - mcu_status.get_imd_ok_high() - - ) { - set_inverter_torques(); - } else if (mcu_status.get_bms_ok_high() && mcu_status.get_imd_ok_high()) { - set_inverter_torques_regen_only(); - } else { - Serial.println("not calculating torque"); - Serial.printf("no brake implausibility: %d\n", mcu_status.get_no_brake_implausability()); - Serial.printf("no accel implausibility: %d\n", mcu_status.get_no_accel_implausability()); - Serial.printf("no accel brake implausibility: %d\n", mcu_status.get_no_accel_brake_implausability()); - Serial.printf("software is ok: %d\n", mcu_status.get_software_is_ok()); - Serial.printf("get bms ok high: %d\n", mcu_status.get_bms_ok_high()); - Serial.printf("get imd ok high: %d\n", mcu_status.get_imd_ok_high()); - set_all_inverters_no_torque(); - } - - break; - } -} - -/* Shared state functinality */ - - -bool check_TS_over_HV_threshold() { - for (uint8_t inv = 0; inv < 4; inv++) { - if (mc_energy[inv].get_dc_bus_voltage() < MIN_HV_VOLTAGE) { - return false; - } - } - return true; -} - -// if TS is below HV threshold, return to Tractive System Not Active -inline void check_TS_active() { - if (!check_TS_over_HV_threshold()) { -#if DEBUG - Serial.println("Setting state to TS Not Active, because TS is below HV threshold"); -#endif - set_state(MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); - inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; - } -} - - -/* Implementation of software shutdown */ -inline void software_shutdown() { - - mcu_status.set_software_is_ok(true); - - // check inputs - // BMS heartbeat has not arrived within time interval - if (timer_bms_heartbeat.check()) { - timer_bms_heartbeat.interval(0); -#if DEBUG - Serial.println("no bms heartbeat"); -#endif - mcu_status.set_software_is_ok(false); - } - - // add BMS software checks - // software ok/not ok action - if (mcu_status.get_software_is_ok()) { - digitalWrite(SOFTWARE_OK, HIGH); //eventually make this HIGH only if software is ok - } else { - digitalWrite(SOFTWARE_OK, LOW); - } - /* Watchdog timer */ - if (timer_watchdog_timer.check()) { - static bool watchdog_state = HIGH; - watchdog_state = !watchdog_state; - digitalWrite(WATCHDOG_INPUT, watchdog_state); - } - -} - -/* Parse incoming CAN messages */ -void parse_telem_can_message(const CAN_message_t &RX_msg) { - CAN_message_t rx_msg = RX_msg; - switch (rx_msg.id) { - case ID_BMS_TEMPERATURES: - bms_temperatures.load(rx_msg.buf); - filtered_max_cell_temp = filtered_max_cell_temp * cell_temp_alpha + (1.0 - cell_temp_alpha) * (bms_temperatures.get_high_temperature() / 100.0) ; - break; - case ID_BMS_VOLTAGES: - bms_voltages.load(rx_msg.buf); - if (bms_voltages.get_low() < PACK_CHARGE_CRIT_LOWEST_CELL_THRESHOLD || bms_voltages.get_total() < PACK_CHARGE_CRIT_TOTAL_THRESHOLD) { - mcu_status.set_pack_charge_critical(true); - } else mcu_status.set_pack_charge_critical(false); - filtered_min_cell_voltage = filtered_min_cell_voltage * cell_voltage_alpha + (1.0 - cell_voltage_alpha) * (bms_voltages.get_low() / 10000.0); - break; - case ID_BMS_COULOMB_COUNTS: bms_coulomb_counts.load(rx_msg.buf); break; - case ID_BMS_STATUS: - bms_status.load(rx_msg.buf); - // BMS heartbeat timer - timer_bms_heartbeat.reset(); - timer_bms_heartbeat.interval(BMS_HEARTBEAT_TIMEOUT); - break; - case ID_DASHBOARD_STATUS: - dashboard_status.load(rx_msg.buf); - /* process dashboard buttons */ - if (dashboard_status.get_torque_mode_btn()) { - switch (mcu_status.get_torque_mode()) { - case 1: - mcu_status.set_max_torque(TORQUE_2); - mcu_status.set_torque_mode(2); break; - case 2: - mcu_status.set_max_torque(TORQUE_3); - mcu_status.set_torque_mode(3); break; - case 3: - mcu_status.set_max_torque(TORQUE_1); - mcu_status.set_torque_mode(1); break; - } - } - if (dashboard_status.get_launch_ctrl_btn()) { - mcu_status.toggle_launch_ctrl_active(); - } - if (dashboard_status.get_mc_cycle_btn()) { - inverter_restart = true; - timer_reset_inverter.reset(); - } - // eliminate all action buttons to not process twice - dashboard_status.set_button_flags(0); - break; - } -} - -void parse_front_inv_can_message(const CAN_message_t &RX_msg) { - CAN_message_t rx_msg = RX_msg; - switch (rx_msg.id) { - case ID_MC1_STATUS: mc_status[0].load(rx_msg.buf); break; - case ID_MC2_STATUS: mc_status[1].load(rx_msg.buf); break; - case ID_MC1_TEMPS: mc_temps[0].load(rx_msg.buf); break; - case ID_MC2_TEMPS: mc_temps[1].load(rx_msg.buf); break; - case ID_MC1_ENERGY: mc_energy[0].load(rx_msg.buf); break; - case ID_MC2_ENERGY: mc_energy[1].load(rx_msg.buf); break; - } -} - -void parse_rear_inv_can_message(const CAN_message_t &RX_msg) { - CAN_message_t rx_msg = RX_msg; - - switch (rx_msg.id) { - case ID_MC3_STATUS: mc_status[2].load(rx_msg.buf); break; - case ID_MC4_STATUS: mc_status[3].load(rx_msg.buf); break; - case ID_MC3_TEMPS: mc_temps[2].load(rx_msg.buf); break; - case ID_MC4_TEMPS: mc_temps[3].load(rx_msg.buf); break; - case ID_MC3_ENERGY: mc_energy[2].load(rx_msg.buf); break; - case ID_MC4_ENERGY: mc_energy[3].load(rx_msg.buf); break; - } -} -//FIXME -inline void power_off_inverter() { - digitalWrite(INVERTER_24V_EN, LOW); - -#if DEBUG - Serial.println("INVERTER POWER OFF"); -#endif -} - - -/* Handle changes in state */ -void set_state(MCU_STATE new_state) { - if (mcu_status.get_state() == new_state) { - return; - } - - // exit logic - switch (mcu_status.get_state()) { - case MCU_STATE::STARTUP: break; - case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: break; - case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: break; - case MCU_STATE::ENABLING_INVERTER: - timer_inverter_enable.reset(); - break; - case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: - // make dashboard stop buzzer - mcu_status.set_activate_buzzer(false); - mcu_status.write(msg.buf); - msg.id = ID_MCU_STATUS; - msg.len = sizeof(mcu_status); - TELEM_CAN.write(msg); - break; - case MCU_STATE::READY_TO_DRIVE: { - inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; - break; - } - } - - mcu_status.set_state(new_state); - - // entry logic - switch (new_state) { - case MCU_STATE::STARTUP: break; - case MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE: break; - case MCU_STATE::TRACTIVE_SYSTEM_ACTIVE: { - inverter_startup_state = INVERTER_STARTUP_STATE::WAIT_SYSTEM_READY; - break; - } - case MCU_STATE::ENABLING_INVERTER: { - Serial.println("MCU Sent enable command"); - timer_inverter_enable.reset(); - break; - } - case MCU_STATE::WAITING_READY_TO_DRIVE_SOUND: - // make dashboard sound buzzer - mcu_status.set_activate_buzzer(true); - mcu_status.write(msg.buf); - msg.id = ID_MCU_STATUS; - msg.len = sizeof(mcu_status); - TELEM_CAN.write(msg); - - timer_ready_sound.reset(); - Serial.println("RTDS enabled"); - break; - case MCU_STATE::READY_TO_DRIVE: - Serial.println("Ready to drive"); - break; - } -} - -inline float float_map(float x, float in_min, float in_max, float out_min, float out_max) -{ - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; -} - -inline void set_inverter_torques_regen_only() { - int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); - int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); - int avg_brake = (brake1 + brake2) / 2; - if (avg_brake > 1500) { - avg_brake = 1500; - } - if (avg_brake < 0) { - avg_brake = 0; - } - torque_setpoint_array[0] = - avg_brake; - torque_setpoint_array[1] = - avg_brake; - torque_setpoint_array[2] = - avg_brake; - torque_setpoint_array[3] = - avg_brake; - for (int i = 0; i < 4; i++) { - if (i < 2) { - torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * 1.33); - } else { - torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * 0.66); - } - - } - for (int i = 0; i < 4; i++) { - if (torque_setpoint_array[i] >= 0) { - } - else { - int16_t max_speed_regen = 0; - for (int i = 0; i < sizeof(torque_setpoint_array); i++) { - - max_speed_regen = (max_speed_regen < mc_status[i].get_speed()) ? mc_status[i].get_speed() : max_speed_regen; - - } - float scale_down = 1; - if (max_speed_regen < 770) { - scale_down = 0; - } else if (max_speed_regen > REGEN_OFF_START_THRESHOLD) { - scale_down = 1; - } else { - scale_down = map(max_speed_regen, 770, REGEN_OFF_START_THRESHOLD, 0, 1); - } - mc_setpoints_command[i].set_speed_setpoint(0); - mc_setpoints_command[i].set_pos_torque_limit(0); - mc_setpoints_command[i].set_neg_torque_limit(max(((int16_t)(torque_setpoint_array[i]) * scale_down) , -2140) ); - - } - } -} - -inline void set_inverter_torques() { - - float max_torque = mcu_status.get_max_torque() / 0.0098; // max possible value for torque multiplier, unit in 0.1% nominal torque - int accel1 = map(round(mcu_pedal_readings.get_accelerator_pedal_1()), START_ACCELERATOR_PEDAL_1, END_ACCELERATOR_PEDAL_1, 0, 2140); - int accel2 = map(round(mcu_pedal_readings.get_accelerator_pedal_2()), START_ACCELERATOR_PEDAL_2, END_ACCELERATOR_PEDAL_2, 0, 2140); - - int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); - int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); - - - // torque values are greater than the max possible value, set them to max - if (accel1 > max_torque) { - accel1 = max_torque; - } - if (accel2 > max_torque) { - accel2 = max_torque; - } - int avg_accel = (accel1 + accel2) / 2; - int avg_brake = (brake1 + brake2) / 2; - if (avg_accel > max_torque) { - avg_accel = max_torque; - } - if (avg_accel < 0) { - avg_accel = 0; - } - if (avg_accel > max_torque) { - avg_accel = max_torque; - } - if (avg_accel < 0) { - avg_accel = 0; - } - if (avg_brake > 1500) { - avg_brake = 1500; - } - if (avg_brake < 0) { - avg_brake = 0; - } - - float rear_power_balance = 0.66; - float front_power_balance = 1.0 - rear_power_balance; - float rear_brake_balance = 0.1; - float front_brake_balance = 1.0 - rear_brake_balance; - - int32_t total_torque = 0; - int32_t total_load_cells = 0; - float front_load_total; - float rear_load_total; - - float attesa_def_split = 0.85; - float attesa_alt_split = 0.5; - float fr_slip_clamped; - float fr_slip_factor = 2.5; // Factor of 5 causes 50/50 split at 20% r/f slip. Lower values allow more slip - float f_torque; - float r_torque; - float rear_lr_slip_clamped; - float lsd_right_split; // Fraction of rear axle torque going to rear right wheel - float lsd_slip_factor = 0.5; - - float avg_speed = 0.0; - for (int i = 0; i < 4; i++) - avg_speed += ((float) mc_status[i].get_speed()) / 4.0; - int16_t start_derating_rpm = 2000; - int16_t end_derating_rpm = 20000; - - const float hairpin_rpm_limit = 5600.0; - const float hairpin_rpm_full = 2800.0; - float hairpin_rpm_factor = 0.0; - const float hairpin_steering_min = 80.0; // degrees - const float hairpin_steering_max = 120.0; // degrees - float steering_calibration_slope = -0.111; - float steering_calibration_offset = 260.0; - // positive steering angle is to the left - float steering_angle = mcu_analog_readings.get_steering_2() * steering_calibration_slope + steering_calibration_offset; - float hairpin_reallocation = 0.0; - float hairpin_steering_factor = 0.0; - - int16_t max_speed; - - switch (dashboard_status.get_dial_state()) { - case 0: - for (int i = 0; i < 4; i++) { - speed_setpoint_array[i] = MAX_ALLOWED_SPEED; - } - launch_state = launch_not_ready; - // standard no torque vectoring - - max_front_power = 19000.0; - max_rear_power = 36000.0; - - torque_setpoint_array[0] = avg_accel - avg_brake; - torque_setpoint_array[1] = avg_accel - avg_brake; - torque_setpoint_array[2] = avg_accel - avg_brake; - torque_setpoint_array[3] = avg_accel - avg_brake; - - for (int i = 0; i < 4; i++) { - if (torque_setpoint_array[i] >= 0) { - if (i < 2) { - torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * front_power_balance); - } else { - torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * rear_power_balance); - } - } else { - if (i < 2) { - torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * front_brake_balance); - } else { - torque_setpoint_array[i] = (int16_t)(torque_setpoint_array[i] * rear_brake_balance); - } - } - } - break; - case 1: - for (int i = 0; i < 4; i++) { - speed_setpoint_array[i] = MAX_ALLOWED_SPEED; - } - launch_state = launch_not_ready; - // Original load cell torque vectoring - - - max_front_power = 19000.0; - max_rear_power = 36000.0; - - - load_cell_alpha = 0.95; - total_torque = 4 * (avg_accel - avg_brake) ; - total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); - if (avg_accel >= avg_brake) { - torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque); - } else { - torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); - torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); - } - break; - case 2: - max_speed = 0; - launch_rate_target = 11.76; - for (int i = 0; i < 4; i++) { - max_speed = max(max_speed, mc_status[i].get_speed()); - } -// max_front_power = 19000.0; -// max_rear_power = 36000.0; - max_front_power = 21760.0; - max_rear_power = 41230.0; - - switch (launch_state) { - case launch_not_ready: - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = (int16_t)(-1 * avg_brake); - speed_setpoint_array[i] = 0; - } - time_since_launch = 0; - launch_speed_target = 0; - - // To enter launch_ready, the following conditions must be true: - // 1. Pedals are not pressed - // 2. Speed is zero - if (avg_accel < LAUNCH_READY_ACCEL_THRESHOLD && avg_brake < LAUNCH_READY_BRAKE_THRESHOLD && max_speed < LAUNCH_READY_SPEED_THRESHOLD) { - launch_state = launch_ready; - } - break; - case launch_ready: - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = 0; - speed_setpoint_array[i] = 0; - } - time_since_launch = 0; - launch_speed_target = 0; - - // Revert to launch_not_ready if brake is pressed or speed is too high - if (avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD || max_speed >= LAUNCH_READY_SPEED_THRESHOLD) { - launch_state = launch_not_ready; - } else { - // Otherwise, check if launch should begin - if (avg_accel >= LAUNCH_GO_THRESHOLD) { - launch_state = launching; - } - } - - break; - case launching: - // Exit launch if accel pedal goes past STOP threshold or brake pedal is pressed - if (avg_accel <= LAUNCH_STOP_THRESHOLD || avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD) { - launch_state = launch_not_ready; - break; - } - - launch_speed_target = (int16_t)((float) time_since_launch / 1000.0 * launch_rate_target * 60.0 / 1.2767432544 * 11.86); - launch_speed_target += 1500; - launch_speed_target = min(20000, max(0, launch_speed_target)); - - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = 2142; - speed_setpoint_array[i] = launch_speed_target; - } - break; - default: - break; - } - - break; - case 3: - max_speed = 0; - launch_rate_target = 12.74; - for (int i = 0; i < 4; i++) { - max_speed = max(max_speed, mc_status[i].get_speed()); - } -// max_front_power = 19000.0; -// max_rear_power = 36000.0; - max_front_power = 21760.0; - max_rear_power = 41230.0; - - switch (launch_state) { - case launch_not_ready: - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = (int16_t)(-1 * avg_brake); - speed_setpoint_array[i] = 0; - } - time_since_launch = 0; - launch_speed_target = 0; - - // To enter launch_ready, the following conditions must be true: - // 1. Pedals are not pressed - // 2. Speed is zero - if (avg_accel < LAUNCH_READY_ACCEL_THRESHOLD && avg_brake < LAUNCH_READY_BRAKE_THRESHOLD && max_speed < LAUNCH_READY_SPEED_THRESHOLD) { - launch_state = launch_ready; - } - break; - case launch_ready: - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = 0; - speed_setpoint_array[i] = 0; - } - time_since_launch = 0; - launch_speed_target = 0; - - // Revert to launch_not_ready if brake is pressed or speed is too high - if (avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD || max_speed >= LAUNCH_READY_SPEED_THRESHOLD) { - launch_state = launch_not_ready; - } else { - // Otherwise, check if launch should begin - if (avg_accel >= LAUNCH_GO_THRESHOLD) { - launch_state = launching; - } - } - - break; - case launching: - // Exit launch if accel pedal goes past STOP threshold or brake pedal is pressed - if (avg_accel <= LAUNCH_STOP_THRESHOLD || avg_brake >= LAUNCH_READY_BRAKE_THRESHOLD) { - launch_state = launch_not_ready; - break; - } - - launch_speed_target = (int16_t)((float) time_since_launch / 1000.0 * launch_rate_target * 60.0 / 1.2767432544 * 11.86); - launch_speed_target += 1500; - launch_speed_target = min(20000, max(0, launch_speed_target)); - - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = 2142; - speed_setpoint_array[i] = launch_speed_target; - } - break; - default: - break; - } - - break; - case 4: - { - // Copy pasted from mode 2 with additional derating for endurance - for (int i = 0; i < 4; i++) { - speed_setpoint_array[i] = MAX_ALLOWED_SPEED; - } - max_front_power = 19000.0; - max_rear_power = 36000.0; - launch_state = launch_not_ready; - // Original load cell torque vectoring - load_cell_alpha = 0.95; - total_torque = 4 * (avg_accel - avg_brake) ; - total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); - - // Derating - float derating_factor = float_map(avg_speed, start_derating_rpm, end_derating_rpm, 1.0, 0.0); - derating_factor = min(1.0, max(0.0, derating_factor)); - - if (avg_accel >= avg_brake) { - torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); - torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); - torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); - torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque * derating_factor); - } else { - torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); - torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); - } - break; - } - case 5: - { - for (int i = 0; i < 4; i++) { - speed_setpoint_array[i] = MAX_ALLOWED_SPEED; - } - launch_state = launch_not_ready; - // Original load cell torque vectoring - - - max_front_power = 21760.0; - max_rear_power = 41240.0; - - - load_cell_alpha = 0.95; - total_torque = 4 * (avg_accel - avg_brake) ; - total_load_cells = mcu_load_cells.get_FL_load_cell() + mcu_load_cells.get_FR_load_cell() + mcu_load_cells.get_RL_load_cell() + mcu_load_cells.get_RR_load_cell(); - if (avg_accel >= avg_brake) { - torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque); - } else { - torque_setpoint_array[0] = (int16_t)((float)mcu_load_cells.get_FL_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[1] = (int16_t)((float)mcu_load_cells.get_FR_load_cell() / (float)total_load_cells * (float)total_torque); - torque_setpoint_array[2] = (int16_t)((float)mcu_load_cells.get_RL_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); - torque_setpoint_array[3] = (int16_t)((float)mcu_load_cells.get_RR_load_cell() / (float)total_load_cells * (float)total_torque / 2.0); - } - break; - } - default: - Serial.println("uh oh"); - break; - } - - - - - //very start check if mc_energy.get_feedback_torque > 0 - //power limit to 80kW - //look at all torques - //look at motor speeds / convert from rpm to angular speed - //torque * speed / 1000 (kW) - // scale down by m/e limits - //lots of variables for documentation purposes - //since torque unit to nominal torque and power conversion are linear, the diff can be applied directly to the torque setpoint value. - if (mc_setpoints_command[0].get_pos_torque_limit() > 0 && mc_setpoints_command[1].get_pos_torque_limit() > 0 - && mc_setpoints_command[2].get_pos_torque_limit() > 0 && mc_setpoints_command[3].get_pos_torque_limit() > 0) { - float mech_power = 0; - float mdiff = 1; - //float ediff = 1; - // float pw_lim_factor = 1.0; - - float voltage_lim_factor = 1.0; - float temp_lim_factor = 1.0; - float accu_lim_factor = 1.0; - - for (int i = 0; i < 4; i++) { - float torque_in_nm = 9.8 * ((float) mc_setpoints_command[i].get_pos_torque_limit()) / 1000.0; - float speed_in_rpm = (float) mc_status[i].get_speed(); - mech_power += 2 * 3.1415 * torque_in_nm * speed_in_rpm / 60.0; - } - - // pw_lim_factor = float_map(mech_power, 40000.0, 55000.0, 1.0, 0); - // pw_lim_factor = max(min(1.0, pw_lim_factor), 0.0); - - voltage_lim_factor = float_map(filtered_min_cell_voltage, 3.5, 3.2, 1.0, 0.2); - voltage_lim_factor = max(min(1.0, voltage_lim_factor), 0.2); - - temp_lim_factor = float_map(filtered_max_cell_temp, 50.0, 58.0, 1.0, 0.2); - temp_lim_factor = max(min(1.0, temp_lim_factor), 0.2); - - accu_lim_factor = min(temp_lim_factor, voltage_lim_factor); - //mech_power /= 1000.0; - - // float current = (ADC1.read_channel(ADC_CURRENT_CHANNEL) - ADC1.read_channel(ADC_REFERENCE_CHANNEL)); - // current = ((((current / 819.0) / .1912) / 4.832) - 2.5) * 1000) / 6.67; - - // - // float dc_power = (mc_energy[0].get_dc_bus_voltage() * current) / 1000; //mc dc bus voltage - - //sum up kilowatts to align - //if mech_power is at 63 kW, it's requesting 80 kW from the motor - //2 kW safety factor for the more accurate motor readings. - //as our effiecency increases say 68 kW would be drawing 80kW from the motor - //as our efficiency decreases say 60 kW would be drawing 80kW from the motor - //so if efficency is at 60kW and we want 63, we'd be drawing more from the battery triggering a safety problem - //so if efficency is at 68 kW, 63 would be drawing less power, which is fine but wasted power. - //if HV DC bus is over 80 kW, it's a violation! - // 1 kW as a second safety factor. - //if (mech_power > MECH_POWER_LIMIT) { - // mdiff = MECH_POWER_LIMIT / mech_power; - // diff = MECH_POWER_LIMIT / mech_power; - //} - // if (dc_power > DC_POWER_LIMIT) { - // ediff = DC_POWER_LIMIT / dc_power; - // } - // if (mech_power > MECH_POWER_LIMIT && dc_power > DC_POWER_LIMIT) { - // diff = (ediff <= mdiff) ? ediff : mdiff; - // } - torque_setpoint_array[0] = (uint16_t) (min((float) torque_setpoint_array[0], max_allowed_torque(max_front_power / 2.0, (float) mc_status[0].get_speed())) * accu_lim_factor); - torque_setpoint_array[1] = (uint16_t) (min((float) torque_setpoint_array[1], max_allowed_torque(max_front_power / 2.0, (float) mc_status[1].get_speed())) * accu_lim_factor); - torque_setpoint_array[2] = (uint16_t) (min((float) torque_setpoint_array[2], max_allowed_torque(max_rear_power / 2.0, (float) mc_status[2].get_speed())) * accu_lim_factor); - torque_setpoint_array[3] = (uint16_t) (min((float) torque_setpoint_array[3], max_allowed_torque(max_rear_power / 2.0, (float) mc_status[3].get_speed())) * accu_lim_factor); - } - - - int16_t max_speed_regen = 0; - for (int i = 0; i < sizeof(torque_setpoint_array); i++) { - - max_speed_regen = (max_speed_regen < mc_status[i].get_speed()) ? mc_status[i].get_speed() : max_speed_regen; - - } - - for (int i = 0; i < 4; i++) { - torque_setpoint_array[i] = max(-2140, min(2140, torque_setpoint_array[i])); - } - - for (int i = 0; i < 4; i++) { - if (torque_setpoint_array[i] >= 0) { - mc_setpoints_command[i].set_speed_setpoint(speed_setpoint_array[i]); - mc_setpoints_command[i].set_pos_torque_limit(min(torque_setpoint_array[i] , 2140)); - mc_setpoints_command[i].set_neg_torque_limit(0); - - } - else { - - float scale_down = 1; - if (max_speed_regen < 770) { - scale_down = 0; - } else if (max_speed_regen > REGEN_OFF_START_THRESHOLD) { - scale_down = 1; - } else { - scale_down = map(max_speed_regen, 770, REGEN_OFF_START_THRESHOLD, 0, 1); - } - mc_setpoints_command[i].set_speed_setpoint(0); - mc_setpoints_command[i].set_pos_torque_limit(0); - mc_setpoints_command[i].set_neg_torque_limit(max(((int16_t)(torque_setpoint_array[i]) * scale_down) , -2140) ); - - } - } - -} - -inline void read_all_adcs() { - if (timer_read_all_adcs.check()) { - - prev_load_cell_readings[0] = mcu_load_cells.get_FL_load_cell(); - prev_load_cell_readings[1] = mcu_load_cells.get_FR_load_cell(); - prev_load_cell_readings[2] = mcu_load_cells.get_RL_load_cell(); - prev_load_cell_readings[3] = mcu_load_cells.get_RR_load_cell(); - - uint16_t adc1_inputs[8]; - ADC1.read_all_channels(&adc1_inputs[0]); - mcu_pedal_readings.set_accelerator_pedal_1(adc1_inputs[ADC_ACCEL_1_CHANNEL]); - mcu_pedal_readings.set_accelerator_pedal_2(adc1_inputs[ADC_ACCEL_2_CHANNEL]); - mcu_pedal_readings.set_brake_pedal_1(adc1_inputs[ADC_BRAKE_1_CHANNEL]); - mcu_pedal_readings.set_brake_pedal_2(adc1_inputs[ADC_BRAKE_2_CHANNEL]); - mcu_analog_readings.set_steering_2(adc1_inputs[ADC_STEERING_2_CHANNEL]); - current_read = adc1_inputs[ADC_CURRENT_CHANNEL] - adc1_inputs[ADC_REFERENCE_CHANNEL]; - float current = ((((current_read / 819.0) / .1912) / 4.832) * 1000) / 6.67; - if (current > 300) { - current = 300; - } else if (current < -300) { - current = -300; - } - mcu_analog_readings.set_hall_effect_current((uint16_t)current * 100); - - - - mcu_load_cells.set_RL_load_cell((uint16_t)((adc1_inputs[ADC_RL_LOAD_CELL_CHANNEL]*LOAD_CELL3_SLOPE + LOAD_CELL3_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[2]*load_cell_alpha)); - - mcu_status.set_brake_pedal_active(mcu_pedal_readings.get_brake_pedal_1() >= BRAKE_ACTIVE); - digitalWrite(BRAKE_LIGHT_CTRL, mcu_status.get_brake_pedal_active()); - - mcu_status.set_mech_brake_active(mcu_pedal_readings.get_brake_pedal_1() >= BRAKE_THRESHOLD_MECH_BRAKE_1); //define in driver_constraints.h (70%) - - uint16_t adc2_inputs[8]; - ADC2.read_all_channels(&adc2_inputs[0]); - mcu_load_cells.set_RR_load_cell((uint16_t)((adc2_inputs[ADC_RR_LOAD_CELL_CHANNEL]*LOAD_CELL4_SLOPE + LOAD_CELL4_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[3]*load_cell_alpha)); - mcu_load_cells.set_FL_load_cell((uint16_t)((adc2_inputs[ADC_FL_LOAD_CELL_CHANNEL]*LOAD_CELL1_SLOPE + LOAD_CELL1_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[0]*load_cell_alpha)); - mcu_load_cells.set_FR_load_cell((uint16_t)((adc2_inputs[ADC_FR_LOAD_CELL_CHANNEL]*LOAD_CELL2_SLOPE + LOAD_CELL2_OFFSET) * (1 - load_cell_alpha) + prev_load_cell_readings[1]*load_cell_alpha)); - mcu_rear_potentiometers.set_pot4(adc2_inputs[SUS_POT_RL]); - mcu_rear_potentiometers.set_pot6(adc2_inputs[SUS_POT_RR]); - mcu_front_potentiometers.set_pot1(adc2_inputs[SUS_POT_FL]); - - uint16_t adc3_inputs[8]; - ADC3.read_all_channels(&adc3_inputs[0]); - mcu_analog_readings.set_glv_battery_voltage(adc3_inputs[ADC_GLV_READ_CHANNEL]); - mcu_front_potentiometers.set_pot3(adc3_inputs[SUS_POT_FR]); - } -} - -inline void read_steering_spi_values() { - if (timer_steering_spi_read.check()) { - mcu_analog_readings.set_steering_1(STEERING.read_steering()); - - } -} - -bool check_all_inverters_system_ready() { - for (uint8_t inv = 0; inv < 4; inv++) { - if (! mc_status[inv].get_system_ready()) { - return false; - } - } - return true; -} - -bool check_all_inverters_quit_dc_on() { - for (uint8_t inv = 0; inv < 4; inv++) { - if (! mc_status[inv].get_quit_dc_on()) { - return false; - } - } - return true; -} - -bool check_all_inverters_quit_inverter_on() { - for (uint8_t inv = 0; inv < 4; inv++) { - if (! mc_status[inv].get_quit_inverter_on()) { - return false; - } - } - return true; -} - -uint8_t check_all_inverters_error() { - uint8_t error_list = 0; //last 4 bits correspond to error bit in status word of each inverter, inverter 1 is rightmost bit; - for (uint8_t inv = 0; inv < 4; inv++) { - if (mc_status[inv].get_error()) { - error_list = error_list | (0x01 << inv); - } - } - if (error_list) { - mcu_status.set_inverters_error(true); - } else { - mcu_status.set_inverters_error(false); - } - return error_list; -} - -inline void set_all_inverters_no_torque() { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_speed_setpoint(0); - mc_setpoints_command[inv].set_pos_torque_limit(0); - mc_setpoints_command[inv].set_neg_torque_limit(0); - } -} - -inline void set_all_inverters_torque_limit(int limit) { - - //const float max_torque = max_torque_Nm / 0.0098; // max possible value for torque multiplier, unit in 0.1% nominal torque - //inverter torque unit is in 0.1% of nominal torque (9.8Nm), max rated torque is 21Nm, so max possible output is 2142 - - if (limit >= 0) { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_pos_torque_limit(limit); - mc_setpoints_command[inv].set_neg_torque_limit(0); - } - } - else { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_pos_torque_limit(0); - mc_setpoints_command[inv].set_neg_torque_limit(limit); - } - } - -} - -inline void set_all_inverters_speed_setpoint(int setpoint) { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_speed_setpoint(setpoint); - } -} - -inline void set_all_inverters_disabled() { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_inverter_enable(false); - mc_setpoints_command[inv].set_hv_enable(false); - mc_setpoints_command[inv].set_driver_enable(false); - mc_setpoints_command[inv].set_remove_error(false); - mc_setpoints_command[inv].set_speed_setpoint(0); - mc_setpoints_command[inv].set_pos_torque_limit(0); - mc_setpoints_command[inv].set_neg_torque_limit(0); - } -} - -inline void set_all_inverters_dc_on(bool input) { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_hv_enable(input); - } -} - -inline void set_all_inverters_driver_enable(bool input) { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_driver_enable(input); - } -} - -inline void set_all_inverters_inverter_enable(bool input) { - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_inverter_enable(input); - } -} - -inline void reset_inverters() { - if (inverter_restart) { - uint8_t reset_bit = !mc_setpoints_command[0].get_remove_error(); - if (timer_reset_inverter.check()) { - inverter_restart = false; - reset_bit = 0; - } - for (uint8_t inv = 0; inv < 4; inv++) { - mc_setpoints_command[inv].set_remove_error(reset_bit); - } - } -} - -/* Read shutdown system values */ -inline void read_status_values() { - /* Measure shutdown circuits' input */ - mcu_status.set_bms_ok_high(digitalRead(BMS_OK_READ)); - mcu_status.set_imd_ok_high(digitalRead(IMD_OK_READ)); - mcu_status.set_bspd_ok_high(digitalRead(BSPD_OK_READ)); - mcu_status.set_software_ok_high(digitalRead(SOFTWARE_OK_READ)); - - /* Measure shutdown circuits' voltages */ - mcu_status.set_shutdown_b_above_threshold(digitalRead(BOTS_OK_READ)); - mcu_status.set_shutdown_c_above_threshold(digitalRead(IMD_OK_READ)); - mcu_status.set_shutdown_d_above_threshold(digitalRead(BMS_OK_READ)); - mcu_status.set_shutdown_e_above_threshold(digitalRead(BSPD_OK_READ)); -} - -// IMU functions -inline void read_imu() { - if (timer_read_imu.check()) { - double sinAngle = sin(VEHICLE_TILT_ANGLE_X); - double cosAngle = cos(VEHICLE_TILT_ANGLE_X); - double accel_x = IMU.regRead(X_ACCL_OUT) * 0.00245; // 0.00245 is the scale - double accel_y = IMU.regRead(Y_ACCL_OUT) * 0.00245; // 0.00245 is the scale - double accel_z = IMU.regRead(Z_ACCL_OUT) * 0.00245; // 0.00245 is the scale - double x_gyro = IMU.regRead(X_GYRO_OUT) * 0.005; // 0.005 is the scale - double y_gyro = IMU.regRead(Y_GYRO_OUT) * 0.005; // 0.005 is the scale - double z_gyro = IMU.regRead(Z_GYRO_OUT) * 0.005; // 0.005 is the scale - double long_accel = ((-accel_y) * cosAngle) + (accel_z * sinAngle); - double lat_accel = accel_x; - double vert_accel = -((accel_z * cosAngle) - (-accel_y * sinAngle)); - double pitch = (y_gyro * cosAngle) + (z_gyro * sinAngle); - double roll = y_gyro; - double yaw = (z_gyro * cosAngle) - (x_gyro * sinAngle); - imu_accelerometer.set_lat_accel((int16_t)(lat_accel * 1000)); // * 0.00245); // 0.00245 is the scale - imu_accelerometer.set_long_accel((int16_t)(long_accel * 1000)); // * 0.00245); // 0.00245 is the scale - imu_accelerometer.set_vert_accel((int16_t)(vert_accel * 1000)); // * 0.00245); // 0.00245 is the scale - // question about yaw, pitch and roll rates? - imu_gyroscope.set_pitch((int16_t)(pitch * 100)); // * 0.005); // 0.005 is the scale, - imu_gyroscope.set_yaw((int16_t)(yaw * 100 )); // * 0.005); // 0.005 is the scale - imu_gyroscope.set_roll((int16_t)(roll * 100)); // * 0.005); // 0.005 is the scale - } -} - -inline void send_CAN_imu_accelerometer() { - if (timer_CAN_imu_accelerometer_send.check()) { - imu_accelerometer.write(msg.buf); - msg.id = ID_IMU_ACCELEROMETER; - msg.len = sizeof(imu_accelerometer); - TELEM_CAN.write(msg); - } -} - -inline void send_CAN_imu_gyroscope() { - if (timer_CAN_imu_gyroscope_send.check()) { - imu_gyroscope.write(msg.buf); - msg.id = ID_IMU_GYROSCOPE; - msg.len = sizeof(imu_gyroscope); - TELEM_CAN.write(msg); - } -} - -inline void calibrate_imu_velocity(double calibrate_to) { - imu_velocity = calibrate_to; -} - -inline void pitch_angle_calibration() { - // double z_accl_sum = 0.0; - // int ctr = 0; - // time_t start_time, current_time; - // double elapsed_time; - // start_time = time(NULL); - // // Serial.println("Calibration Starts Now"); FOR DEBUGING PURPOSES - // while (1) { - // delay(50); - // z_accl_sum += ((IMU.regRead(Z_ACCL_OUT) * 0.00245)); - // ctr++; - // current_time = time(NULL); - // elapsed_time = difftime(current_time, start_time); - // if (elapsed_time >= 5) break; // Code runs for 5 seconds. Change for desired duration. - // } - // // Serial.println("Calibration Has Ended"); - // double avg_z_accl = z_accl_sum / ctr; - // pitch_calibration_angle = std::acos(avg_z_accl / ACCL_DUE_TO_GRAVITY); -} - -inline void calculate_pedal_implausibilities() { - // FSAE EV.5.5 - // FSAE T.4.2.10 - if (mcu_pedal_readings.get_accelerator_pedal_1() < MIN_ACCELERATOR_PEDAL_1 || mcu_pedal_readings.get_accelerator_pedal_1() > MAX_ACCELERATOR_PEDAL_1) { - mcu_status.set_no_accel_implausability(false); -#if DEBUG - Serial.println("T.4.2.10 1"); -#endif - } - else if (mcu_pedal_readings.get_accelerator_pedal_2() > MAX_ACCELERATOR_PEDAL_2 || mcu_pedal_readings.get_accelerator_pedal_2() < MIN_ACCELERATOR_PEDAL_2) { - mcu_status.set_no_accel_implausability(false); -#if DEBUG - Serial.println("T.4.2.10 2"); -#endif - } - // check that the pedals are reading within 10% of each other - // T.4.2.4 - else if (fabs((mcu_pedal_readings.get_accelerator_pedal_1() - START_ACCELERATOR_PEDAL_1) / (END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) - - (mcu_pedal_readings.get_accelerator_pedal_2() - START_ACCELERATOR_PEDAL_2) / (END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2)) > 0.1) { -#if DEBUG - Serial.println("T.4.2.4"); - Serial.printf("pedal 1 - %f\n", (mcu_pedal_readings.get_accelerator_pedal_1() - START_ACCELERATOR_PEDAL_1) / (END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1)); - Serial.printf("pedal 2 - %f\n", (mcu_pedal_readings.get_accelerator_pedal_2() - START_ACCELERATOR_PEDAL_2) / (END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2)); -#endif - mcu_status.set_no_accel_implausability(false); - } - else { - mcu_status.set_no_accel_implausability(true); - } - - // BSE check - // EV.5.6 - // FSAE T.4.3.4 - if (mcu_pedal_readings.get_brake_pedal_1() < MIN_BRAKE_PEDAL_1 || mcu_pedal_readings.get_brake_pedal_1() > MAX_BRAKE_PEDAL_1) { - mcu_status.set_no_brake_implausability(false); - } - else if (mcu_pedal_readings.get_brake_pedal_2() > MIN_BRAKE_PEDAL_2 || mcu_pedal_readings.get_brake_pedal_2() < MAX_BRAKE_PEDAL_2) { //negative slope for brake 2 - mcu_status.set_no_brake_implausability(false); - } else if (fabs((mcu_pedal_readings.get_brake_pedal_1() - START_BRAKE_PEDAL_1) / (END_BRAKE_PEDAL_1 - START_BRAKE_PEDAL_1) - - (START_BRAKE_PEDAL_2 - mcu_pedal_readings.get_brake_pedal_2()) / (START_BRAKE_PEDAL_2 - END_BRAKE_PEDAL_2)) > 0.25) { - mcu_status.set_no_brake_implausability(false); - } - else { - mcu_status.set_no_brake_implausability(true); - } - - // FSAE EV.5.7 - // APPS/Brake Pedal Plausability Check - if ( - ( - (mcu_pedal_readings.get_accelerator_pedal_1() > ((END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) / 4 + START_ACCELERATOR_PEDAL_1)) - || - (mcu_pedal_readings.get_accelerator_pedal_2() > ((END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2) / 4 + START_ACCELERATOR_PEDAL_2)) - ) - && mcu_status.get_mech_brake_active() - ) - { - mcu_status.set_no_accel_brake_implausability(false); - } - else if - ( - (mcu_pedal_readings.get_accelerator_pedal_1() < ((END_ACCELERATOR_PEDAL_1 - START_ACCELERATOR_PEDAL_1) / 20 + START_ACCELERATOR_PEDAL_1)) - && - (mcu_pedal_readings.get_accelerator_pedal_2() < ((END_ACCELERATOR_PEDAL_2 - START_ACCELERATOR_PEDAL_2) / 20 + START_ACCELERATOR_PEDAL_2)) - ) - { - mcu_status.set_no_accel_brake_implausability(true); - } - if (mcu_status.get_no_accel_implausability() && mcu_status.get_no_brake_implausability() && mcu_status.get_no_accel_brake_implausability()) { - pedal_implausability_duration = 0; - } -} - -inline float max_allowed_torque(float maxwatts, float rpm) { - float angularspeed = (abs(rpm) + 1) / 60 * 2 * 3.1415; - float maxnm = min(maxwatts / angularspeed, 20); - return maxnm / 9.8 * 1000; -} - -void setup() { - // no torque can be provided on startup - - - mcu_status.set_max_torque(0); - mcu_status.set_torque_mode(0); - mcu_status.set_software_is_ok(true); - - set_all_inverters_disabled(); - - - // IMU set up - IMU.regWrite(MSC_CTRL, 0xC1); // Enable Data Ready, set polarity - delay(20); - IMU.regWrite(FLTR_CTRL, 0x504); // Set digital filter - delay(20); - IMU.regWrite(DEC_RATE, 0), // Disable decimation - delay(20); - - pinMode(BRAKE_LIGHT_CTRL, OUTPUT); - - // change to input if comparator is PUSH PULL - pinMode(INVERTER_EN, OUTPUT); - pinMode(INVERTER_24V_EN, OUTPUT); - - pinMode(WATCHDOG_INPUT, OUTPUT); - // the initial state of the watchdog is high - // this is reflected in the static watchdog_state - // starting high - digitalWrite(WATCHDOG_INPUT, HIGH); - pinMode(SOFTWARE_OK, OUTPUT); - digitalWrite(SOFTWARE_OK, HIGH); - - pinMode(ECU_CLK, OUTPUT); - pinMode(ECU_SDI, INPUT); - pinMode(ECU_SDO, OUTPUT); - -#if DEBUG - Serial.begin(115200); -#endif - FRONT_INV_CAN.begin(); - FRONT_INV_CAN.setBaudRate(500000); - REAR_INV_CAN.begin(); - REAR_INV_CAN.setBaudRate(500000); - TELEM_CAN.begin(); - TELEM_CAN.setBaudRate(500000); - FRONT_INV_CAN.enableMBInterrupts(); - REAR_INV_CAN.enableMBInterrupts(); - TELEM_CAN.enableMBInterrupts(); - FRONT_INV_CAN.onReceive(parse_front_inv_can_message); - REAR_INV_CAN.onReceive(parse_rear_inv_can_message); - TELEM_CAN.onReceive(parse_telem_can_message); - delay(500); - -#if DEBUG - Serial.println("CAN system and serial communication initialized"); -#endif - - - // these are false by default - mcu_status.set_bms_ok_high(false); - mcu_status.set_imd_ok_high(false); - - digitalWrite(INVERTER_24V_EN, HIGH); - digitalWrite(INVERTER_EN, HIGH); - mcu_status.set_inverters_error(false); - - - - // present action for 5s - delay(5000); - - set_state(MCU_STATE::TRACTIVE_SYSTEM_NOT_ACTIVE); - mcu_status.set_max_torque(TORQUE_3); - mcu_status.set_torque_mode(3); - - - /* Set up total discharge readings */ - //setup_total_discharge(); - -} - -void loop() { - FRONT_INV_CAN.events(); - REAR_INV_CAN.events(); - TELEM_CAN.events(); - - read_all_adcs(); - //read_steering_spi_values(); - read_status_values(); - read_imu(); - - send_CAN_mcu_status(); - send_CAN_mcu_pedal_readings(); - send_CAN_mcu_load_cells(); - send_CAN_mcu_potentiometers(); - send_CAN_mcu_analog_readings(); - send_CAN_imu_accelerometer(); - send_CAN_imu_gyroscope(); - send_CAN_inverter_setpoints(); - - // /* Finish restarting the inverter when timer expires */ - check_all_inverters_error(); - reset_inverters(); - // - // /* handle state functionality */ - state_machine(); - software_shutdown(); - - - - if (timer_debug.check()) { - Serial.println("ERROR"); - Serial.println(check_all_inverters_error()); - Serial.println(mc_energy[0].get_dc_bus_voltage()); - Serial.println(mc_temps[0].get_diagnostic_number()); - Serial.println(mc_temps[1].get_diagnostic_number()); - Serial.println(mc_temps[2].get_diagnostic_number()); - Serial.println(mc_temps[3].get_diagnostic_number()); - Serial.println(); - Serial.println(mcu_pedal_readings.get_accelerator_pedal_1()); - Serial.println(mcu_pedal_readings.get_accelerator_pedal_2()); - Serial.println(mcu_pedal_readings.get_brake_pedal_1()); - Serial.println(mcu_pedal_readings.get_brake_pedal_2()); - //calculate_pedal_implausibilities(); -// Serial.println(mcu_status.get_no_accel_implausability()); -// Serial.println(mcu_status.get_no_brake_implausability()); -// Serial.println(mcu_status.get_no_accel_brake_implausability()); -// int brake1 = map(round(mcu_pedal_readings.get_brake_pedal_1()), START_BRAKE_PEDAL_1, END_BRAKE_PEDAL_1, 0, 2140); -// int brake2 = map(round(mcu_pedal_readings.get_brake_pedal_2()), START_BRAKE_PEDAL_2, END_BRAKE_PEDAL_2, 0, 2140); -// Serial.println(brake1); -// Serial.println(brake2); - Serial.println(); - Serial.println("LOAD CELLS"); - Serial.println(mcu_load_cells.get_FL_load_cell()); - Serial.println(mcu_load_cells.get_FR_load_cell()); - Serial.println(mcu_load_cells.get_RL_load_cell()); - Serial.println(mcu_load_cells.get_RR_load_cell()); - Serial.println("SUS POTS"); - Serial.println(mcu_front_potentiometers.get_pot1()); - Serial.println(mcu_front_potentiometers.get_pot3()); - Serial.println(mcu_rear_potentiometers.get_pot4()); - Serial.println(mcu_rear_potentiometers.get_pot6()); - - Serial.println(torque_setpoint_array[0]); - Serial.println(torque_setpoint_array[1]); - Serial.println(torque_setpoint_array[2]); - Serial.println(torque_setpoint_array[3]); - Serial.println("MOTOR TEMPS"); - Serial.println(mc_temps[0].get_motor_temp()); - Serial.println(mc_temps[1].get_motor_temp()); - Serial.println(mc_temps[2].get_motor_temp()); - Serial.println(mc_temps[3].get_motor_temp()); - Serial.println(mc_temps[3].get_igbt_temp()); - Serial.println("IMU"); - Serial.println(imu_accelerometer.get_vert_accel()); - Serial.println(imu_gyroscope.get_yaw()); - Serial.println("dial"); - Serial.println(dashboard_status.get_dial_state()); - } - -} diff --git a/src/test_can_interface.cpp b/src/test_can_interface.cpp index ad178a463..dc811b57b 100644 --- a/src/test_can_interface.cpp +++ b/src/test_can_interface.cpp @@ -15,7 +15,7 @@ InverterInterfaceType fr_inv(&CAN2_txBuffer, ID_MC2_SETPOINTS_COMMAND); InverterInterfaceType rl_inv(&CAN2_txBuffer, ID_MC3_SETPOINTS_COMMAND); InverterInterfaceType rr_inv(&CAN2_txBuffer, ID_MC4_SETPOINTS_COMMAND); -CANInterfaces CAN_interfaces = {&fl_inv, &fr_inv, &rl_inv, &rr_inv, 0, 0}; +CANInterfaces CAN_interfaces = {&fl_inv, &fr_inv, &rl_inv, &rr_inv, 0, 0, 0}; void init_can_interface() { diff --git a/test/test_interfaces/dashboard_interface_test.h b/test/test_interfaces/dashboard_interface_test.h index 5d361df83..168dd511b 100644 --- a/test/test_interfaces/dashboard_interface_test.h +++ b/test/test_interfaces/dashboard_interface_test.h @@ -45,7 +45,7 @@ void test_dashboard_unpacking_can_message(void) TEST_ASSERT_EQUAL(current_state.start_button, dash_interface.startButtonPressed() ); TEST_ASSERT_EQUAL(current_state.mark_button, dash_interface.specialButtonPressed() ); - TEST_ASSERT_EQUAL(current_state.mode_button , dash_interface.torqueButtonPressed() ); + TEST_ASSERT_EQUAL(current_state.mode_button , dash_interface.torqueModeButtonPressed() ); TEST_ASSERT_EQUAL(current_state.motor_controller_cycle_button, dash_interface.inverterResetButtonPressed() ); TEST_ASSERT_EQUAL(current_state.launch_ctrl_button , dash_interface.launchControlButtonPressed() ); TEST_ASSERT_EQUAL(current_state.torque_mode_button , dash_interface.torqueLoadingButtonPressed() ); diff --git a/test/test_systems/drivetrain_system_test.h b/test/test_systems/drivetrain_system_test.h index 3a6281b27..ad48cb90e 100644 --- a/test/test_systems/drivetrain_system_test.h +++ b/test/test_systems/drivetrain_system_test.h @@ -71,7 +71,8 @@ class InverterMock uint16_t dc_bus_voltage() { return voltage_; }; bool dc_quit_on() { return dc_quit_on_; } - bool quit_inverter_on() { return quit_inverter_on_; } + bool get_quit_dc_on() { return dc_quit_on_; } + bool get_quit_inverter_on() { return quit_inverter_on_; } }; TEST(DrivetrainSystemTesting, test_drivetrain_startup) @@ -163,17 +164,18 @@ TEST(DrivetrainSystemTesting, test_drivetrain_inverter_comms) // torque_setpoint_nm_ // speed_setpoint_rpm_ // ensure that without ticking the inverters dont get the data - EXPECT_EQ(inv_fl.speed_setpoint_rpm_, 0.0); - EXPECT_EQ(inv_fl.torque_setpoint_nm_, 0); + // as of 3/13/24 this isnt being handled by the drivetrain rn + // EXPECT_EQ(inv_fl.speed_setpoint_rpm_, 0.0); + // EXPECT_EQ(inv_fl.torque_setpoint_nm_, 0); - EXPECT_EQ(inv_fr.torque_setpoint_nm_, 0); - EXPECT_EQ(inv_fr.speed_setpoint_rpm_, 0); + // EXPECT_EQ(inv_fr.torque_setpoint_nm_, 0); + // EXPECT_EQ(inv_fr.speed_setpoint_rpm_, 0); - EXPECT_EQ(inv_rl.torque_setpoint_nm_, 0); - EXPECT_EQ(inv_rl.speed_setpoint_rpm_, 0); + // EXPECT_EQ(inv_rl.torque_setpoint_nm_, 0); + // EXPECT_EQ(inv_rl.speed_setpoint_rpm_, 0); - EXPECT_EQ(inv_rr.torque_setpoint_nm_, 0); - EXPECT_EQ(inv_rr.speed_setpoint_rpm_, 0); + // EXPECT_EQ(inv_rr.torque_setpoint_nm_, 0); + // EXPECT_EQ(inv_rr.speed_setpoint_rpm_, 0); SysClock clock; auto micros = 1000000; dt.tick(clock.tick(micros)); @@ -191,16 +193,18 @@ TEST(DrivetrainSystemTesting, test_drivetrain_inverter_comms) EXPECT_EQ(inv_rr.speed_setpoint_rpm_, 1003.0); // testing to ensure that these extra general commands dont get through the period filter - micros += (20 * 1000); - dt.tick(clock.tick(micros)); - dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); - dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); - dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); - EXPECT_EQ(inv_rl.cmd_general_count_, 1); - micros += (20 * 1000); - dt.tick(clock.tick(micros)); - dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); - EXPECT_EQ(inv_rl.cmd_general_count_, 2); + // as of 3/13/24, the inverter sending rate is being handled at the inverter interface + // TODO decide if we want to add this back into the drivetrain + // micros += (20 * 1000); + // dt.tick(clock.tick(micros)); + // dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); + // dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); + // dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); + // EXPECT_EQ(inv_rl.cmd_general_count_, 1); + // micros += (20 * 1000); + // dt.tick(clock.tick(micros)); + // dt.command_drivetrain({{1000.0, 1001.0, 1002.0, 1003.0}, {2000.0, 2001.0, 2002.0, 2003.0}}); + // EXPECT_EQ(inv_rl.cmd_general_count_, 2); } // TODO test commanding of drivetrain to ensure that the data is getting accross correctly #endif /* DRIVETRAIN_SYSTEM_TEST */ diff --git a/test/test_systems/pedals_system_test.h b/test/test_systems/pedals_system_test.h index 573b896d1..5f91ec2c7 100644 --- a/test/test_systems/pedals_system_test.h +++ b/test/test_systems/pedals_system_test.h @@ -73,8 +73,8 @@ TEST(PedalsSystemTesting, test_accel_and_brake_percentages_implausibility) TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activation) { - AnalogConversion_s test_accel1_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_brake_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel1_val = {2000, 0.6, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {3000, 1.0, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.1}, 0.0f); auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); @@ -91,8 +91,8 @@ TEST(PedalsSystemTesting, test_accel_and_brake_pressed_at_same_time_and_activati TEST(PedalsSystemTesting, test_implausibility_duration) { - AnalogConversion_s test_accel1_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; - AnalogConversion_s test_brake_val = {1000, 0.3, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_accel1_val = {2000, 0.6, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; + AnalogConversion_s test_brake_val = {3000, 1.0, AnalogSensorStatus_e::ANALOG_SENSOR_GOOD}; PedalsSystem pedals({100, 100, 3000, 3000, 0.1}, {100, 100, 3000, 3000, 0.1}, 0.0f); auto data = pedals.evaluate_pedals(test_accel1_val, test_accel1_val, test_brake_val, test_brake_val, 1000); diff --git a/test/test_systems/state_machine_test.h b/test/test_systems/state_machine_test.h index 6e563c9f4..e8f706fa0 100644 --- a/test/test_systems/state_machine_test.h +++ b/test/test_systems/state_machine_test.h @@ -4,7 +4,6 @@ #include #include "MCUStateMachine.h" -// TODO @ben class DrivetrainMock { @@ -25,6 +24,7 @@ class DrivetrainMock bool drivetrain_error_occured() { return drivetrain_error_; }; void command_drivetrain(const DrivetrainCommand_s &data){}; + void disable_no_pins() {}; }; void handle_startup(MCUStateMachine &state_machine, unsigned long sys_time, DrivetrainMock &drivetrain, PedalsSystem &pedals, DashboardInterface &dash_interface)