From fe69d716652295055d7c332d3af96e5dcf248959 Mon Sep 17 00:00:00 2001 From: lukestroh <66746219+lukestroh@users.noreply.github.com> Date: Thu, 7 Dec 2023 15:40:15 -0800 Subject: [PATCH 01/21] configured motor to have limit switches, e-stop. Updated README --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 79872 -> 77824 bytes ClearCoreVelocityController.atsln | 6 --- README.md | 50 ++++++++++++++++++++- include/EthUDP.h | 10 +++-- include/clearpathmc.h | 9 +++- main.cpp | 28 +++++++----- src/ClearPathMC.cpp | 33 +++++++++++++- src/EthUDP.cpp | 38 +++++++++++++--- 8 files changed, 143 insertions(+), 31 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index ad715d94d7b2e0884ff074264aa5e1c0deed40ff..c3b33d31090255e23797ed90d107a3a8c8b89288 100644 GIT binary patch delta 3832 zcmc&$3s6+o8NUBzcUf584?*#<@=!z;mt9yviQE-fAB4veV$@O<5F?{T3mBD$ENv2T z##-4waI3*I?kF^=#>btA&X~+9$+T)33AG*DX)T?$lTMRqs)L!tsc!#&7f=$6b|y1D zcfbA5`Oo9t^Zk!=9RF51hExZPc&Z_wBRf(Jf*_br+v7z4Q$kKn@ti+ky`~NRuG|sl zLhBsr)DiA`Jf2{ZdO{jOO9+FPoqzF?$1bwnnT)DUJ7LJ89dafB2CPf8#eNSI8B zAn-e)NyZRT3G)PDjWC5OGYNAEGYH89o@^x9NiY#Y2$_U?+(E@c_4$M>LL`B83rXe> z;z!!7%kQv9CfP<5OGo4~lFJECkF<+OmUr0WvY(~O3c_=Qm4sD<)r9X6))06_E^DdI zTNy15+-r`5?H%@c&`2+YR{pz>_h75P#RNuN$&()|fcx{APW z7$u#_W73&8CY|R^M0z;&G3ngTq_Z3)o$Ew;6|+7e=}hiKr1L~B-$r`W$jf-#|Bv)o z-fJ6u3d>QvOl>79umM!8!F{@X90(0lhIENy*ytFR6OY3oo!%Lihy9v#bvCWkjl05{ zpc?OlwZe~Zd-yZBJ32M+ht$IXBM5juwBXwZYkY&1yOA2vIYp29_(U!HGOvz~*kpA* zRldYs@yosIG*jymeidJ+V*dGU>dRi0eOV#Czhwgse3qyb$6g1FpFT^)!NQAR!}JoK zj>FSS0wA5V=CPg`-X2{!DXtT!;KeI*ed|me1Vz8@I_O>^wT)w%$n#7F(Pp(|pkJ4jYAU@gN^PS0g0l*Y-QerzO4ap^d{bG6mTkjY_z zoG^H0jh!kt)YzzRgi__s@>PJvi&7p<@gTxp`>FLDbxxt2o)sjge_yz zAUsdn?Sesg^jHIp*h;9OQ8hHbN~pz|A4iJy^kunH(k`K%ma7)lQ|AV%r_sy8Yo-Tx zu01#WS<8DDo~k(5#J-}fpkX^`fNQDfvQ21OXIXy$@Il2nleeu5mj!|BhysV=C@!Ww z=bD`5-oVF(atb2OyR%2e;&AyjUwGACQ$rGKIWTuqg<`d;gj+7d>CPTMkP5d!`hgpF- z4>-5!75(}lz?I7+MQU)vLo2Ae=v_&t?Y;2WPYz~N^BKcMh4y6>L@%C)@V_+NcjfgB zgugvSc;j-Yryp>p7#COY{f@#DRj%kVOLP~X^bZw9+Iij&_T$~Azf#CzN6WKV(VqAq zF8vhg^&Z1?g6U(P%a1WgBfaTb{Y4Ij5+$zS>s1xu$+mvwR>JVsPCt zwF0}p^!Fd(TArB%Iaqo-8Je-&KQpm_c`Q!M3+ z$Y`mgib%-+4v5H$`@+W=c914Ok zHDAvFrFID`Z^wi*>#DcZ*VeCnzPdVX<8z|~`__f8waZg0YfSSiEmcNCjX5LJkYUQo zGGtrUZ!l!mRBkY5XVlb~E!k;CqtTLO3o5` zt>|nwD%%df@AqAMI}YxjglR`2{8wJ%d$fRXG~0PB8BIrG+3f>A(feFgEqEP>ycAnr zy+)4oJMwLx?~Zc{9;JA?0Laujw105YYh})WV}-AwX-dTzNj*+&AoV2WMBg)_vj1P; zH?EJJ#7}wSR;?&|R1iCnD;)yNeeL3yUvVxSE8gkU3M2RtnCOC&b-@@myq7NnZyo*B zYrkI-eeh8KigkSj|5S}$22e_L@KpeH$M=l;o=RFkB=c+pob^Q-4!EM^?s!D-LYUYN{GmK4XLbj;l;f#kXp)H4M{Y+Eo%IXRS~8n>ozkKdq$(@E50Xj!LHK0j6A zJwIr)uRL%>@ zAx|EN0u^0(f@QZ3RGq0Xqi=^1!a-I?K~s+i;zLK`M@h4S&ZP{OmFj=_`r#FO)|`z1 zJ;*~*F!WeE5y$UH;2!(P+q#}8T3fP=9=Bg#8>Lh?orfix+1&Dt3`p*ETgY+0q3)A1 K!7bwD9O=JXub^T8 delta 3984 zcmchadr*|u6~Om=yR5sgC30aaw{=0=^mq6z~BmtEtiY zK-nW*O-!0DK9Y=6qhA8G6Prcth-O4&)M}eaY^zQu`iEs8qei9Y>_UP{95d7O!jF6I zx%az|bM8I&V!0u++>`CrgU}QkgCqzL* zKP3-plt6eABZ)L3nczB3`$%>lQVt#>%Sl!d@x)jnh=?RiL@1#n9=p2fe?qy2$^*sz zcX_D6LKIbuA5s}OL>@gLkD)w_;Ep&YHkpG8(@9|>c)DWu({%AGj zYlyW({eX^_u%6@%#1;Y{LDh(b*2cf=#&G4TZZM&R|EVSYrW^l_@tpFK&yI_1l|gq ztZ7*ao_r$7T!}SlT?#&%e4bs3^nC7A{sk!tKz!&U7pz{!ml(%m$T-;E5rtis!^FIR z%OFnBdLux>CaA&InX9B$&qr|uN4-OvEt_<;UiL-Q-S zc*cKPQ^9D^W5xwFIw}I-xR|kUC)hfDAY9d2vFY=BQ^ziOl-%BPeB2qpGg7^BGxd`* zihF$=pM%Ei`QrNtp`di0z>tB#lsxiZ2#M`v3>eXnYe9$3!oiSrv2uvBRRSo`5@WGX z7MeAMaoD15Zo{YUo!)X!;QZn^AqtydCVb1p7?Ennio&Mce9^fQ{k7-U%hS-)0tVIj7ol;Nr*AJxAlB_I~%J%XDy(Gjis-7(Jr4B&?y&UvA5u& z5+;(CcM{jA_>SevfD@W`O|x}Hfwwq&o+!2YX`bCT^f7N0Uv5huV@>i7*osi>@5#}R z&C6RayvZC75p)-W)7U>u>=Q)*HRERhf9=5$fp;?{v43nI$a%ZwjKYU=&e2NJQ7Fuj z@%7G)(O_NSD@tb=;5UGC4!!Fqi<+_5-KFB|n`|Sz z5>G@lelz;_`P&xExg%fpMj$pXpG>3p;LM5?>S-e=BzoshG`3g!yK$VA;PTZ$qM^ni zZE;iz_B8??t5W00jIiMbjq&KwxWa^+q#I+l??ufCAN;sci@NRIxNw^c&N{WbKh)!a zgBS6-Eh@D%ao+jGc!>VEs4b$Eva})8wOMHVEwDs@Q;Q+vz_5_+*x=7&T2`* z&+6jFOeY)EOMa}!gt{awt4|0s>Xx#x`+2eb&sM?D z=$x$U&pgWbjhA&{z(X?e+&jsFSoPN~!J9+AEBQKj$r0i|ju_n3gAWU?;xNE(%&fr; zAgaW*fO`-AeR$vFr@L}7{lj+j!T`K(%@Z|8LF&f`c`$Lj;@4`{(|hLYTYf^z_^X0g zx2ID`<(t!$!X0)J_{QfaF`o$_vX8Fxtfl*ADLurOJ8zx=AK5b8A(dTZ%Tz&t^{mC- zp%RaN6e1K3Yn7kHC%&U+msHf2l+RmKURk=VX05KYs>*6hSHnuD+hB++w%pdp{7%t| z22(c){IAOY%web!@7OO3I2-Fl&!fIC>5Q#X`&~i2+guEuYX+xAc{46-_eaYWi?w4D z>d$?Gn>zj=-aHp9I}`wd{afKsAgHi?MT(fx*#QSYI22Cm`wHyp2o{6Cy$NzYNLC4s zubhjoT?w@5l`w&P>e&pinF1unnlv^;C^+!Ql!I-#0bm0BbZTzfbQw6LtqQOJemy0` z_MQ?Jw7P=<_(-TxbJp8d>H$TE9UcI-3O7ix6=}iWrt*aUV2Q=#ygl)slgFXyk^*n* zifoP;Fz^Ge?Xf3p1vuk!PNNjcaJ(r*w3zaoH@5e4H@5Q;Ty>!i$6XF~Taubz&hG@< RwMfvmIl{pKZ5AC(>%Zt$8ioJ> diff --git a/ClearCoreVelocityController.atsln b/ClearCoreVelocityController.atsln index 03a5fd2..2abd7f9 100644 --- a/ClearCoreVelocityController.atsln +++ b/ClearCoreVelocityController.atsln @@ -9,8 +9,6 @@ Project("{E66E83B9-2572-4076-B26E-6BE79FF3018A}") = "ClearCore", "..\libClearCor EndProject Project("{E66E83B9-2572-4076-B26E-6BE79FF3018A}") = "LwIP", "..\LwIP\LwIP.cppproj", "{C373696C-5D45-4B91-AD62-A21552361596}" EndProject -Project("{E66E83B9-2572-4076-B26E-6BE79FF3018A}") = "ManualVelocity", "..\..\..\..\..\..\Program Files (x86)\Teknic\ClearCore-Library 1.0\Microchip_Examples\ClearPathModeExamples\ClearPath-MC_Series\ManualVelocity\ManualVelocity.cppproj", "{8E27B935-12EA-4FA1-8A5F-53EB06696B1A}" -EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|ARM = Debug|ARM @@ -29,10 +27,6 @@ Global {C373696C-5D45-4B91-AD62-A21552361596}.Debug|ARM.Build.0 = Debug|ARM {C373696C-5D45-4B91-AD62-A21552361596}.Release|ARM.ActiveCfg = Release|ARM {C373696C-5D45-4B91-AD62-A21552361596}.Release|ARM.Build.0 = Release|ARM - {8E27B935-12EA-4FA1-8A5F-53EB06696B1A}.Debug|ARM.ActiveCfg = Debug|ARM - {8E27B935-12EA-4FA1-8A5F-53EB06696B1A}.Debug|ARM.Build.0 = Debug|ARM - {8E27B935-12EA-4FA1-8A5F-53EB06696B1A}.Release|ARM.ActiveCfg = Release|ARM - {8E27B935-12EA-4FA1-8A5F-53EB06696B1A}.Release|ARM.Build.0 = Release|ARM EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/README.md b/README.md index c466bdf..1f3f3fe 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,52 @@ # ProjectTemplate This repository contains a basic ClearCore project that can be used as a template for new application development. -Place this repository rooted in the same parent directory as libClearCore and LwIP to properly find include files and libraries. \ No newline at end of file +Place this repository rooted in the same parent directory as libClearCore and LwIP to properly find include files and libraries. + +## Requirements: + +### ETHERNET COMMUNICATION: +1. The PC should be running software capable of sending and receiving UDP +packets. See `udp_client.py` for simple testing. + + +### MOTOR: +1. A ClearPath motor must be connected to Connector M-0. +2. The connected ClearPath motor must be configured through the MSP software +for Manual Velocity Control mode (In MSP select Mode>>Velocity>>Manual +Velocity Control, then hit the OK button). +3. In the MSP software: + Define a Max Clockwise and Counter-Clockwise (CW/CCW) Velocity (On the + main MSP window fill in the textboxes labeled "Max CW Velocity (RPM)" + and "Max CCW Velocity (RPM)"). Any velocity commanded outside of this + range will be rejected. + Set the Velocity Resolution to 2 (On the main MSP window check the + textbox labeled "Velocity Resolution (RPM per knob count)" 2 is + default). This means the commanded velocity will always be a multiple + of 2. For finer resolution, lower this value and change + velocityResolution in the sketch below to match. + Set Knob Direction to As-Wired, and check the Has Detents box (On the + main MSP window check the dropdown labeled "Knob Direction" and the + checkbox directly below it labeled "Has Detents"). + On the main MSP window set the dropdown labeled "On Enable..." to be + "Zero Velocity". + Set the HLFB mode to "ASG-Velocity w/Measured Torque" with a PWM carrier + frequency of 482 Hz through the MSP software (select Advanced>>High + Level Feedback \[Mode\]... then choose "ASG-Velocity w/Measured Torque" + from the dropdown, make sure that 482 Hz is selected in the "PWM Carrier + Frequency" dropdown, and hit the OK button). + +### LIMIT SWITCHES: +1. Limit switches should be connected to the I0 and I1 inputs on the controller. + + +### EMERGENCY STOP: +1. Emergency stop should be connected to the DI-6 input on the controller. + +Links: +ClearCore Documentation: https://teknic-inc.github.io/ClearCore-library/ +ClearCore Manual: https://www.teknic.com/files/downloads/clearcore_user_manual.pdf + + +Copyright (c) 2020 Teknic Inc. This work is free to use, copy and distribute under the terms of +the standard MIT permissive software license which can be found at https://opensource.org/licenses/MIT \ No newline at end of file diff --git a/include/EthUDP.h b/include/EthUDP.h index ed0f5e5..db0a19a 100644 --- a/include/EthUDP.h +++ b/include/EthUDP.h @@ -17,8 +17,8 @@ class EthUDP { const int local_port; // Host IP address, port - IpAddress remote_ip = IpAddress(169, 254, 93, 234); - const int remote_port = 8888; + IpAddress remote_ip; + const int remote_port; public: // Data buffer @@ -32,8 +32,10 @@ class EthUDP { EthernetUdp udp; EthUDP(); - EthUDP(IpAddress _ip); - EthUDP(IpAddress _ip, int _port); + EthUDP(IpAddress _local_ip); + EthUDP(IpAddress _local_ip, int _local_port); + EthUDP(IpAddress _local_ip, IpAddress _remote_ip); + EthUDP(IpAddress _local_ip, int _local_port, IpAddress _remote_ip, int _remote_port); ~EthUDP(); // Public methods diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 2d0e422..7dfb86f 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -31,6 +31,13 @@ class ClearPathMC { const double velocity_resolution = 1.0; int motor_id; + // Limit switch pins + ClearCorePins limit_switch_pin_neg = CLEARCORE_PIN_IO0; + ClearCorePins limit_switch_pin_pos = CLEARCORE_PIN_IO1; + + // Emergency stop pin + ClearCorePins emergency_stop_pin = CLEARCORE_PIN_DI6; + bool check_for_faults(); void handle_motor_faults(); void assert_HLFB(); @@ -45,7 +52,7 @@ class ClearPathMC { void begin(); void get_position(); - void get_velocity(); + float get_velocity(); void set_position(double pos); void set_velocity(double vel); diff --git a/main.cpp b/main.cpp index 50188bb..9f6227d 100644 --- a/main.cpp +++ b/main.cpp @@ -35,6 +35,13 @@ * from the dropdown, make sure that 482 Hz is selected in the "PWM Carrier * Frequency" dropdown, and hit the OK button). * + * LIMIT SWITCHES: + * 1. Limit switches should be connected to the I0 and I1 inputs on the controller. + * + * + * EMERGENCY STOP: + * 1. Emergency stop should be connected to the DI-6 input on the controller. + * * Links: * ** ClearCore Documentation: https://teknic-inc.github.io/ClearCore-library/ * ** ClearCore Manual: https://www.teknic.com/files/downloads/clearcore_user_manual.pdf @@ -49,16 +56,14 @@ #include "EthUDP.h" #include "ClearPathMC.h" -// Set a static address for the ClearCore Controller -IpAddress ip = IpAddress(169, 254, 97, 177); - -EthUDP eth(ip); - +// Set static addresses for the ClearCore Controller +IpAddress local_ip = IpAddress(169, 254, 97, 177); +IpAddress remote_ip = IpAddress(169, 254, 57, 209); +EthUDP eth(local_ip, remote_ip); ClearPathMC motor0(0); - void set_up_serial(void) { /* Set up Serial communication with computer for debugging */ ConnectorUsb.Mode(Connector::USB_CDC); @@ -74,6 +79,7 @@ void set_up_serial(void) { int main(void) { set_up_serial(); + eth.begin(); motor0.begin(); @@ -91,10 +97,12 @@ int main(void) { // Move to target velocity (blocking) motor0.move_at_target_velocity(); - // Send data to the ROS2 node. - eth.send_packet(motor0.target_velocity); - - // Check position/limits, if boundary, set motor speed to zero + // Get current velocity + float motor_vel = motor0.get_velocity(); + ConnectorUsb.SendLine(motor_vel); + ConnectorUsb.SendLine(motor0.target_velocity); + // Send velocity data to the ROS2 node. + eth.send_packet(motor_vel); } } \ No newline at end of file diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index a634b5f..3ac4ada 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -39,6 +39,19 @@ void ClearPathMC::begin() { // Enable the motor motor.EnableRequest(true); ConnectorUsb.SendLine("Motor enabled."); + + // Set the limit switches + if (!motor.LimitSwitchNeg(limit_switch_pin_neg)) { + ConnectorUsb.SendLine("Error in enabling negative limit switch. Proceed with caution."); + } + if (!motor.LimitSwitchPos(limit_switch_pin_pos)) { + ConnectorUsb.SendLine("Error in enabling positive limit switch. Proceed with caution."); + } + + // Set up the emergency stop + if (!motor.EStopConnector(emergency_stop_pin)) { + ConnectorUsb.SendLine("Error in enabling emergency stop switch. Proceed with caution."); + } // Wait for HLFB assert_HLFB(); @@ -95,8 +108,24 @@ void ClearPathMC::assert_HLFB() { } -void ClearPathMC::get_velocity() { - +float ClearPathMC::get_velocity() { + /* + Get the current velocity from the HLFB + The duty cycle scales as a percentage of the maximum motor speed configured in the currently selected operating mode. + - 5% duty cycle = 0% max speed + - 95% duty cycle = 100% max speed + HLFB output deasserts (i.e., 0% duty cycle, "off", non-conducting) when the motor is disabled or shutdown. + */ + MotorDriver::HlfbStates hlfb_state = motor.HlfbState(); + if (hlfb_state == MotorDriver::HLFB_HAS_MEASUREMENT) { + // Get the measured speed as a percent of Max Speed + float hlfb_vel_percent = motor.HlfbPercent(); + float hlfb_vel = hlfb_vel_percent * max_velocity_CW; + return hlfb_vel; + } + else { + return 0.0; + } } void ClearPathMC::set_velocity(double vel) { diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 29186d8..05db68a 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -11,26 +11,50 @@ EthUDP::EthUDP(): /* Initialize class variables */ local_ip(169, 254, 97, 177), - local_port {8888} + local_port {8888}, + remote_ip(169, 254, 57, 209), + remote_port {8888} { } -EthUDP::EthUDP(IpAddress _ip): - local_ip(_ip), - local_port {8888} +EthUDP::EthUDP(IpAddress _local_ip): + local_ip(_local_ip), + local_port {8888}, + remote_ip(169, 254, 57, 209), + remote_port {8888} { } -EthUDP::EthUDP(IpAddress _ip, int _port): - local_ip(_ip), - local_port(_port) +EthUDP::EthUDP(IpAddress _local_ip, int _local_port): + local_ip(_local_ip), + local_port(_local_port), + remote_ip(169, 254, 57, 209), + remote_port {8888} { } +EthUDP::EthUDP(IpAddress _local_ip, IpAddress _remote_ip): + local_ip(_local_ip), + local_port {8888}, + remote_ip(_remote_ip), + remote_port {8888} +{ + +} + +EthUDP::EthUDP(IpAddress _local_ip, int _local_port, IpAddress _remote_ip, int _remote_port): + local_ip(_local_ip), + local_port(_local_port), + remote_ip(_remote_ip), + remote_port(_remote_port) +{ + +} + EthUDP::~EthUDP() {} void EthUDP::begin(void) { From 4b4f906ca1d7e3f2bb1820343c2db6ce2f066ef5 Mon Sep 17 00:00:00 2001 From: lukestroh <66746219+lukestroh@users.noreply.github.com> Date: Tue, 9 Jan 2024 11:40:24 -0800 Subject: [PATCH 02/21] added system interrupts, estop works. Reconfigured output message to include system state --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 77824 -> 92160 bytes ClearCoreVelocityController.cppproj | 6 ++ include/EthUDP.h | 5 +- include/clearpathmc.h | 22 +++--- include/interrupts.h | 34 ++++++++++ include/system.h | 24 +++++++ main.cpp | 74 +++++++++++++++++++-- src/ClearPathMC.cpp | 35 ++++++---- src/EthUDP.cpp | 22 ++++-- 9 files changed, 185 insertions(+), 37 deletions(-) create mode 100644 include/interrupts.h create mode 100644 include/system.h diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index c3b33d31090255e23797ed90d107a3a8c8b89288..af1e823e26167f6d440eb7a00b3c40c9390e7a29 100644 GIT binary patch delta 7075 zcmd^D30PD|5`O*WV1{83R1gqw7!*Nd@E~H4OAeJ)PK~Y!F^+&j0s{_%3PwRpG#*iz zQeTWWYGRJ{;6;dTVn*X8nnMYi)wt1Ubid^LHrW{0B&)lTt$H(qfZ&>Z+2h;o`TptZ ze%lEp4<><`uVrtF-|^wB zx;H-4AXEq~A`%ggpt){X_d@t0+!4_T4}?D=0?{AQ0}+Jii3mXSMGQhrKm;R{h(JUr zf_Qpi{Ug>?yg9C#Ghhcb!m;k>px0qN3^B^lcEx%)Vj!ZAqmSmqBJI_%Q zM8s%B5+VgbbH+I8u~?6zrKTa_>FA^ZdZe@4wW)6swqcwiN;{+7gFINKxT3tY_dq+9 zDhIOa2OVQbtuBarsg;L@TID#0)Jo6Ws}q@P(*Ytw%Sl_=|@y+9KLVLA_VLJqI-)fy(O+wY;l*I_5pT>HLgMJ3s;nNnxmAdR?^;O5vR&kx?3I2avMp)P53SXy$ zDvv5<4GJFR2e&+`*t1aKS?#(KJ5&hN^z9y1-3?B4Aa!(;K#`w;hZDK z!=d?1e=)4M>HekfRUSt>dxbZBV4cthvLtSQNAM&5l}iEABT@sB_Tonkp_Y#Die|)h_gQMR~I@6Sj5)(sZ37J4i(BV z{4!cOJQd=mG5HK?WT>ycdU*^D#YHQgLc*c65qdc}cd2_R5{K*xi`T2-utmOIQrIda z95bSW?8``WKC45bQM?)Lq8w;Oxy@V=H-{_0+Q8*;W%%>SY?7Z2C|{u3{AJai+DDsd zWy-H78+x{WBiK}IUU)iJ%2^Pdrx$X&zw~;kKH=~ysx$9-P5FeA(^O3*myCl=IAx`9 zM8)O~v^P!o(mV$r16Rn+e&oFR;>%~3z7??a+J58I*J*B#T-+NA?n(hy%;~tbWKQ*o ze1krg_0B>$vJp9mi3svNkQMABmx$uv3G(IG3vUFcSVLa+gq5SGwDew3|6o(rQ}LPW z`Vx0I88VD*L7vrC<2)CKR4{Sc@VYXkIIwa(-qhP9Q@PKVdZAXyAJFwn~*o8nx&CyZcjo62C z)%%fXK-}JMjAk%t+OYp72^HHeg-w(8<48;>+k`?t3n#NZJUkx1*njrU_dP;nAD6~7 zy_0~N^s#H^;72{yy@0@8G zX08H_+QOCNmKAaXMt$mE5Y@+UrtyP-Bj;zi(0etf>wH6RUmr9uXEQZbrFlh`N<&^5 z*amG0F6Sz7ss%TsL}D;|{$EnvE30k=ytg-GO6Z?432BflNOE$WTv`Wj2ghVX6dj!$ zxuve1oLifd!y%BpI#y^7XP#pEWKs5ut4l%FN`hObd4SF2BPLF_u|^fEVS=p}i_!`c zesw{%^U)u|3=6|wC6lzqk0S>JmiR1H6s~Ust&!aCjrc5*lD!5M2^k9Z0QKWQ&-anY zVfcroo6SyoVtdw2_bq?F@>R&ubAFl&_r2Nm`Fx`*OJvq?Q_q)^ze-)bc;m89x6WL( zam^P;@f6rJjMe4%2yvk-)M`BBW3rUzj<$4k=p;V`Mf!s7U@xBI>J5B%I{w+Kl!7L0 zbvHp%i$OEvh+4WX48UQePVq*D8*9ip#>25EYvhU4+aZL#KoMRS+%byYkI7L=Accu` zadlyihD?r9GAz*Ql_rlTLW-*1PqAZgb77`@l%q?r5Q#%jDU%WjYoXkQvOp|i>r}=| z0l$x43eA)#Nbe$lbk31_U@jmV+GYAN6aL;ZQk+pMxIlm20}^NY!vIs?5r^dV+ZfuA zitFk0k7g;bnDJ=Jy6T(*{Yodnw2%FSA!?>#&ZZt_m^NE8-iG8y(!nG1+KyHeZ_UA+ zhVBys9m@qvPNtLs-N?bfPCMcZ>~557xFM*((2x2-;OsxJTDUd)${pVYOUoaEqvd{( z8S2$gQNtJqi{>OiVyI25nUg5D=S9DGpeL4elL2gN2#baQrGC=81Gvsq>5_WyJ;M_D5Ov2nm6wRhk66In&{A;ogPF70CFqfoBOK~o~3_QR`{KQ%+_nIf^i~fRabjHxda4AmN zL^rWY4&wwjg%@^v!sSki7r6C0y>vFvO!?@hV2vyckC5h*RsfemwQ zGS(>NlWnF+NoM=3@*bx7AFy!txyW|zV3IL+-V4cs(Tc=?-B(zf<#GIq@3Q z9^WHgJyGStTRBHq&dkF8s&3n!VEN?z<<)`H8DrDtUgD zbD8$&6?nJ*rfk;!ZE?MGtUjc>tC>@*zBr|AxSe&#y#s{(ds?F;uCBYRn&b3sjKyg` z*7MMO++Xzn{wh;av}n`0{f+wWR!IVpIdl>p!3 zQR9U;In%Hr=lvz#Qd(M^S7W;gos(Diii%q5vEd8K}3OZMp15&DX+L9zaSSQ zKyF@s!Av1a%d#}-rMR{fbJ3gvGfa>0YQnqXF;@Qp4@Zv=au}|ElWj0dl);hNBgK&? zLuCK91CR|p$C}9IQT4vCy5$w#ZuP!hT0Lqrq}_2F*ZBYh-0J{r!t)jhLwXkj|M9~A zuQTA=?HM@0SU)BNxv&8EI;NMfMa>2Yt7PnuP~yT0g>hOI8_>>t@VYB`ab*YFZrav| z{jF=84ieJ5kWWXBT^Zy6*&U;w*ulJiveB0t=yY?+SW`uDFhC*SDJ1cMZ>?beL;mFLid-+XUL+GlHR*~73Z8o_Z*mVYpN*!NfE*hwU@KQ8gpIP^!w)b4=mZn2><{9 delta 4277 zcmeH}dsI}{6^HL0W`+?YfC8eR4&noW!C@F)N|BLa1OWx>JBmESN0TN5bV&eVVpM7p zAF$QiAh8Cmc4=yq+d66Qa*|SO4q1^Uc|J zpT|9WpS{ny_EwF(UA;cZKTwh+H*mYFs|$rhAZA}~kV?B(N-CZ>l1o*~=`-cJDuQZ8 zqRtbDwq7XpAPVR}ZveIPDug_`=Q+5i9Ews4h68Uf5Cnm6AUcJj^Z@-qA0Xc0kFp<# z1fwJ=PZ|Ir1Vn(rAPk7kMwAV?}x>2jNp;kH8Iw+FgSxaUD`6CK6% zD7X3byiBxve77x~`2xlh6L+x^&#OQ!_z74IUIH(JSHK#u7OVs7K^>?E3^o7}3Y&n; zGqXau$|IO|@~a*(Nxy>b*I++zfCJzlI0W7TZ-aNhyWlWr0q=qHfOVe1HOC>G0j=N^ zI1R+4&!IdE+CWE-?gGjW!SBFD@DcbJ{2p8apMcBYQ_v2sfIk2+-qoH`$k!l$4*n!b z$~MnAs{ebd9p3m3-r{ZE7gUWJuJEbQ7NLv2FW>U1pci?GZ;i)tD0Bb|d)vF>Va;f{ za=mYaiuL7T>O|4j9vH@p%U96*e6u`LEpmjb{UbHUAiU4H8JT=^*?ieJnuaP91KTu; zKHxlQSD@17&9??;Xh>{aq6aV5=jf21&V4|Ps&wjmQ}eha@^dA@WYqo;%o1I4QUldb z3gPM0EVXFCZ>L)8%hT`7N>hf~Hh#Xm=HYX*4o<4K?($t7)%;FS?aA9y&S#caTnK+x z!p6s^ttM(@OJa>|aw9uQOgxrb>RX!XNG?sJ0NL(K>6~XC;Qd8Yw%liX;D$YZv1zkL zkKuy3{@iFDN^hX|YrJKFk8kO$e*Yf0{_-nneOH#;+I#M@#5J*&y2*VEv!!CG927}Q zq{Y&+QYp+|h>iGJsRY_*q-4npXTWUSOQkZUD}q!_KT@$ha)+|RIO}_RC(NoF4X5-K zZgZ|%*d#vU7OixKsmcB4(29z#=nSH8OLolCt6;`CeIkyD8Pt z>bqxvwDp*G*Ae0nRS*wb71aBnTlcpOmP*G3tNd^<>#`7s8Bz|6TU)E=HMM0NWgesq zI&?`zYx(Q79mpoGT0fO9)erq9W18#iq}Vw{&Gv?;@{@HQ>7UG0gvQ84dxM|J{rj*{>4>^$VVMXs+qGV#yBdX`#I&v4e>uaYmGD3z;C&L{kOzMQn4E z)Pr3OzS-umm3S@z%HG~matAJ0|R zUHO+fM-5lFt^x=uV0}zmr~6h_RVIv!Nig9y#F>mH1Jk-sC{84EoHM}cR|F!!LS7!p zr*nKXQ}I&5=A6Q$^z*9nEc$WTqO!RQii)BZ%KA*2;rug178IM}3*rimhT<4=tifzb zNH8SE&6{tCEiRZJlV~n3j)_Z*G8&C>3C1ce`C~x9Q!usr8s73IEE|dk>%Hz#f;+Dg z&uI(6c29LR+=q>dal?AI|6Qze+s1&&-C1;ZjL$gZ;hd@oaW68)j2y8b<_*XM3*z4f zDHfTKhKw*G%;S(3agd`?y9}AbDVxLkJtA%BnnIVh5odpT2I2N0S8qSB1h;pP5^|$e z#R>oH?IFJ41=cf&{-1Kn0iC>eD(U#-t}x})-x!DUTrX7Sw0-3qGtk)v?R-RAAQmL;%~CJEvS&3m8}D7)KiK@yc>h-$ zj!esUr2Uz%B>Ov0?&douH+Owj>zv-5=)89UIkFQPi)Oc>fSEP2^;jJk>_KslS6%)J6Msl;@6N$ZwIH??cC_Nse}-QT%1~ zY|g3<*dIXWNnSXb%DH^bAo(8ys8HG0If?Gr>fy5Wq5c!6jo-ul_Z2Bmbp*KyV}o5| z-S4->3$s(}PlR)z@`Zd^OUc|&7vT6s5WPVzGj`}mE1Pr_>?j#XS84wfltq&!Pg7)6;07yk;6jH0Rk# z-kMJ7o2FAj^ZXQ&>#`|SesVg^+19dv-g(*Ojq8MPd0hoEDDyBLut7@su1sK zRk^0^Jh<1+9qx4?ymNAn{4;4p_3A zoThv{gi_@48I&Sy;jKI diff --git a/ClearCoreVelocityController.cppproj b/ClearCoreVelocityController.cppproj index fde2f9c..b193c1f 100644 --- a/ClearCoreVelocityController.cppproj +++ b/ClearCoreVelocityController.cppproj @@ -262,6 +262,12 @@ compile + + compile + + + compile + compile diff --git a/include/EthUDP.h b/include/EthUDP.h index db0a19a..104f74a 100644 --- a/include/EthUDP.h +++ b/include/EthUDP.h @@ -6,6 +6,7 @@ */ #include "ClearCore.h" #include "EthernetUdp.h" +#include "system.h" #ifndef ETHUDP_H_ #define ETHUDP_H_ @@ -41,8 +42,8 @@ class EthUDP { // Public methods void begin(); void read_packet(); - char* construct_data_msg(float data); // // Set true if using DHCP to configure the local IP address - void send_packet(float data); + char* construct_data_msg(slidersystem::SystemStatus system_status, float data); + void send_packet(slidersystem::SystemStatus system_status, float data); }; #endif /* ETHUDP_H_ */ \ No newline at end of file diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 7dfb86f..a8222c0 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -15,8 +15,19 @@ // To disable automatic fault handling, #define HANDLE_MOTOR_FAULTS (0) #define HANDLE_MOTOR_FAULTS (0) -class ClearPathMC { +extern volatile bool neg_lim_switch_flag; +extern volatile bool pos_lim_switch_flag; +extern volatile bool e_stop_flag; + +class ClearPathMC { private: + // Limit switch pins + DigitalIn& limit_switch_pin_neg = ConnectorIO0; + DigitalIn& limit_switch_pin_pos = ConnectorIO1; + + // Emergency stop pin + DigitalIn& emergency_stop_pin = ConnectorDI6; + // Motor MotorDriver& motor = ConnectorM0; @@ -31,13 +42,6 @@ class ClearPathMC { const double velocity_resolution = 1.0; int motor_id; - // Limit switch pins - ClearCorePins limit_switch_pin_neg = CLEARCORE_PIN_IO0; - ClearCorePins limit_switch_pin_pos = CLEARCORE_PIN_IO1; - - // Emergency stop pin - ClearCorePins emergency_stop_pin = CLEARCORE_PIN_DI6; - bool check_for_faults(); void handle_motor_faults(); void assert_HLFB(); @@ -56,7 +60,7 @@ class ClearPathMC { void set_position(double pos); void set_velocity(double vel); - void move_at_target_velocity(); + void move_at_target_velocity(bool hard_stop = false); void stop(); }; diff --git a/include/interrupts.h b/include/interrupts.h new file mode 100644 index 0000000..0db65a3 --- /dev/null +++ b/include/interrupts.h @@ -0,0 +1,34 @@ +/* + * interrupts.h + * + * Created: 1/5/2024 5:14:42 PM + * Author: Luke Strohbehn + */ + + +#ifndef INTERRUPTS_H_ +#define INTERRUPTS_H_ + +#include "system.h" + +extern volatile bool neg_lim_switch_flag; +extern volatile bool pos_lim_switch_flag; +extern volatile bool e_stop_flag; +extern volatile slidersystem::SystemStatus system_status; + +void emergency_stop_callback() { + e_stop_flag = true; + system_status = slidersystem::E_STOP; +} + +void neg_lim_switch_callback() { + neg_lim_switch_flag = true; + system_status = slidersystem::POS_LIM; +} + +void pos_lim_switch_callback() { + pos_lim_switch_flag = true; + system_status = slidersystem::NEG_LIM; +} + +#endif /* INTERRUPTS_H_ */ \ No newline at end of file diff --git a/include/system.h b/include/system.h new file mode 100644 index 0000000..c238c94 --- /dev/null +++ b/include/system.h @@ -0,0 +1,24 @@ +/* + * system.h + * + * Created: 1/5/2024 10:07:56 PM + * Author: Luke Strohbehn + */ + + +#ifndef __SYSTEM_H__ +#define __SYSTEM_H__ + +namespace slidersystem +{ + enum SystemStatus { + SYSTEM_OK, + SYSTEM_STANDBY, + NEG_LIM, + POS_LIM, + E_STOP, + }; + +} // namespace system + +#endif /* __SYSTEM_H__ */ \ No newline at end of file diff --git a/main.cpp b/main.cpp index 9f6227d..71c8208 100644 --- a/main.cpp +++ b/main.cpp @@ -55,6 +55,11 @@ #include "ClearCore.h" #include "EthUDP.h" #include "ClearPathMC.h" +#include "system.h" + +volatile bool neg_lim_switch_flag = false; +volatile bool pos_lim_switch_flag = false; +volatile bool e_stop_flag = false; // Set static addresses for the ClearCore Controller IpAddress local_ip = IpAddress(169, 254, 97, 177); @@ -64,6 +69,9 @@ EthUDP eth(local_ip, remote_ip); ClearPathMC motor0(0); +// System state variable +volatile slidersystem::SystemStatus system_state = slidersystem::SYSTEM_STANDBY; + void set_up_serial(void) { /* Set up Serial communication with computer for debugging */ ConnectorUsb.Mode(Connector::USB_CDC); @@ -76,15 +84,45 @@ void set_up_serial(void) { } } +void reset_slider(void) { + /* Reset the linear slider to position 0, where base is closest to the motor. + Run the motor at a low constant velocity until the negative limit switch is trigged + and send a message via Ethernet to ROS2. */ + motor0.target_velocity = -30; + while (!neg_lim_switch_flag) { + motor0.move_at_target_velocity(); + eth.send_packet(system_state, motor0.target_velocity); + } + motor0.target_velocity = 0; + motor0.move_at_target_velocity(true); +} + +bool read_switch(DigitalIn& switch_pin, bool& interrupt_flag) { + if (interrupt_flag) { + bool reading = switch_pin.State(); + static bool change_pending = false; + if (!reading) { + change_pending = true; + } + if (reading && change_pending) { + interrupt_flag = false; + change_pending = false; + return true; + } + } + return false; +} + int main(void) { set_up_serial(); - eth.begin(); motor0.begin(); + reset_slider(); + while (true) { - // Read data from the ROS2 node. + // Read data from the ROS2 hardware interface. eth.read_packet(); // If new data, parse for new motor control @@ -94,15 +132,37 @@ int main(void) { eth.new_data = false; } + // Limit switch check + if (neg_lim_switch_flag) { + motor0.target_velocity = 0; + motor0.move_at_target_velocity(true); + //system_state = slidersystem::SystemStatus::NEG_LIM; + } + if (pos_lim_switch_flag) { + motor0.target_velocity = 0; + motor0.move_at_target_velocity(true); + //system_state = slidersystem::SystemStatus::POS_LIM; + } + + // E stop check + while (e_stop_flag) { + motor0.target_velocity = 0; + motor0.move_at_target_velocity(true); + //system_state = slidersystem::SystemStatus::E_STOP; + ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); + eth.send_packet(system_state, motor0.target_velocity); + Delay_ms(1000); + } + // Move to target velocity (blocking) motor0.move_at_target_velocity(); - // Get current velocity - float motor_vel = motor0.get_velocity(); - ConnectorUsb.SendLine(motor_vel); + //// Get current velocity + //float motor_vel = motor0.get_velocity(); + //ConnectorUsb.SendLine(motor_vel); ConnectorUsb.SendLine(motor0.target_velocity); - // Send velocity data to the ROS2 node. - eth.send_packet(motor_vel); + // Send status, velocity data to the ROS2 node. + eth.send_packet(system_state, motor0.target_velocity); } } \ No newline at end of file diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 3ac4ada..6472223 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -6,6 +6,7 @@ */ #include "ClearPathMC.h" +#include "interrupts.h" ClearPathMC::ClearPathMC() {} @@ -40,19 +41,13 @@ void ClearPathMC::begin() { motor.EnableRequest(true); ConnectorUsb.SendLine("Motor enabled."); - // Set the limit switches - if (!motor.LimitSwitchNeg(limit_switch_pin_neg)) { - ConnectorUsb.SendLine("Error in enabling negative limit switch. Proceed with caution."); - } - if (!motor.LimitSwitchPos(limit_switch_pin_pos)) { - ConnectorUsb.SendLine("Error in enabling positive limit switch. Proceed with caution."); - } + // Enable pin interrupts + limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::RISING, true); + limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::RISING, true); + emergency_stop_pin.InterruptHandlerSet(&emergency_stop_callback, InputManager::RISING, true); + + - // Set up the emergency stop - if (!motor.EStopConnector(emergency_stop_pin)) { - ConnectorUsb.SendLine("Error in enabling emergency stop switch. Proceed with caution."); - } - // Wait for HLFB assert_HLFB(); @@ -142,12 +137,22 @@ void ClearPathMC::set_velocity(double vel) { } -void ClearPathMC::move_at_target_velocity() { +void ClearPathMC::move_at_target_velocity(bool hard_stop) { /* Move the motor at the set target velocity */ // Check motor status check_for_faults(); + // If at negative limit switch, don't let target velocity be negative + if (neg_lim_switch_flag && target_velocity < 0) { + target_velocity = 0; + } + + // If at positive limit switch, don't let target velocity be positive + if (pos_lim_switch_flag && target_velocity > 0) { + target_velocity = 0; + } + // Determine which order the quadrature must be sent by determining if the // new velocity is greater or less than the previously commanded velocity // If greater, Input A begins the quadrature. If less, Input B begins the @@ -162,6 +167,10 @@ void ClearPathMC::move_at_target_velocity() { } for (int32_t i = 0; i < velocity_difference; ++i) { + // If a flag is raised via interrupts + if ((e_stop_flag || neg_lim_switch_flag || pos_lim_switch_flag) && (!hard_stop)) { + return; + } if (target_velocity > current_velocity) { // Toggle Input A to begin the quadrature signal motor.MotorInAState(true); diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 05db68a..4d305f3 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -101,24 +101,34 @@ void EthUDP::read_packet(void) { } } -char* EthUDP::construct_data_msg(float data) { - /* Construct the message to send to the ROS2 Node on the host computer */ +char* EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float data) { + /* Construct the message to send to the ROS2 Node on the host computer + + https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c + */ + memset(&msg_buf[0], 0, sizeof(msg_buf)); + char status_buf[2]; + sprintf(status_buf, "%d", system_status); char data_buf[10]; sprintf(data_buf, "%f", data); - char header[19] = "{\"servo_velocity\":"; + char status_header[11] = "{\"status\":"; + char vel_header[13] = "\"servo_rpm\":"; char footer[2] = "}"; - strcpy(msg_buf, header); + strcpy(msg_buf, status_header); + strcat(msg_buf, status_buf); + strcat(msg_buf, ","); + strcat(msg_buf, vel_header); strcat(msg_buf, data_buf); strcat(msg_buf, footer); return msg_buf; } -void EthUDP::send_packet(float data) { +void EthUDP::send_packet(slidersystem::SystemStatus system_status, float data) { /* Send a packet */ - char* msg = construct_data_msg(data); + char* msg = construct_data_msg(system_status, data); udp.Connect(remote_ip, remote_port); udp.PacketWrite(msg); From db566691caaecd3bbd2cf52d49b0fe306070e24d Mon Sep 17 00:00:00 2001 From: lukestroh Date: Tue, 2 Apr 2024 13:44:35 -0700 Subject: [PATCH 03/21] added calibration loop to microcontroller --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 92160 -> 89600 bytes Release/Makefile | 167 +++++++++++++++++++++ Release/makedep.mk | 12 ++ include/EthUDP.h | 32 +++- include/clearpathmc.h | 19 ++- include/interrupts.h | 4 +- include/system.h | 1 + main.cpp | 73 ++++++--- src/ClearPathMC.cpp | 78 ++++++++-- src/EthUDP.cpp | 104 ++++++++----- 10 files changed, 403 insertions(+), 87 deletions(-) create mode 100644 Release/Makefile create mode 100644 Release/makedep.mk diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index af1e823e26167f6d440eb7a00b3c40c9390e7a29..98f9d58ea3d69b0cbc1197176fe240841f705690 100644 GIT binary patch delta 4145 zcmcIn3v`oJ68`7UqiNGeLPJR(wrTo`eUY|l+M+^eQ{JTyD7#`UElsgSb}blG9?~vH z*OkSdHnZ?+MOP}~?oz4tM^h0bXw_v^L`4q^s0beQpa<(xchBOZJCmerK|Si8_2zt; z`RC4^d+*%2bMNgqqUzYE>biQOK@bE5;hWLXQ6d5mlAoxK7u9Vi+HaV8C$17V!mIKZ zVivStLv>mLsg6bvjUtp0xNSaB4Iz$TBqS2lgam?_kV=?Jun-~%R}kU}nS@$G3L%Z~ zHQ7oh$~Gqwoj}MXTqy|oLNqbige*c5VKRaAMl*!CF-Yt}ip{d0014Z4055upGCmCgV|+McF}L z9&v?wxOc-ix2ZD%ZeuI#I(8Ymj=#sdE`(&`UB~s8yN+f5gX`?ctsuJpQ7p~FG zRkBxc!^CX$GGgMczze#=N{)Q&Fj{a#U-=W%<&o%YQ8t-QfwXE8%$MgIj{=%aJC(c< z0`^Q~Ps~Gia-|A*-*C^`+lV<#pdziD1eNmqgfAdeD5me31a7H%l$hPPW__h{Mo^?3 zBu32%fmBAXNI`RaG8r#~H*k&XO6<)yYs#sLHzHfmK1U3LiaZ(%;y}zSP;yXnEgZzv zlPYmYVb$JGqE9&{Sg&w4@DR~mRHfnkFrDDH)=kV)1S<0AT|+cnPAi-yxH>eCm>z!4KLX|7`r7@AVnOjh2II{LNUL|cN2{@psAZ)oxo{}0W zas|56D?|M|q?D`tO-KS1i~e2im$)5V9PB!+%)eXIq+s~84o|>c4+y36)@Z#Q{ z(55Z-cFtVBhg>*-(@WZWXDK}vnvsHLWO*$x8cwN!(OgL*=8Tjal!V)fxtqXcpneZT ztb>YM42`EM1_wWj{T&`mo91cpSL{d8yl^y)`H{_3rg(#4UH`eBE!h=hjqgyis%Q+@52S0;_W;!!~w=?8|ikxlx0fy5fsdEXKoCx|H(YoWAOO zyK~(e>dkXxc^^MI;O&ESu)3bNv5@ zLqVkB!;&+8Aa*SNq=h4vBbx6Gp`2$e@tpJo( zmcXRoCaE$0-!32pI zIU^MxY)Qq2kQmvw!2zC3r)Cm)JtpXR_&u)9oR4MerlFWss^Pl{U+H*J)QgA_;&E-( zd?jDk{4p}pmLTgk7x)iDblXo?PF&rZmUN-Nv$6leldpHT7G9ntd50HE z>tkuv7WHJJug8qNJ&6jMlKgznHK65tH6O1^;V8E6xECFR8YyljMBwm%1z+EFNZznx zHAs)wf(BptizSM89yjLF>iBjOg;H?(n;m%nC8hqJfTI8Z&-AaCrw(L_y&feFpN^5O zFBueAd_EjP@Vd7i!OvQwV`@rsz6#x+Ia`S^3bc5#Sf1Cn1ocdHU7i<)*m z@Ofm6^8Qs3Xs}<`wt4tm#HL5D*?rpWpvOvS&Vq$gtn^#wDk`?;b8JhC z3vx;dT_rjB_F}8e?rL=97c|~Zht$QnW-hraE#a-gORW^xnv3ks#d$f+&9=10oV+}j zHK(yTzd6ThZL~I(SeGqZ)>LqbnTH=Z2Vn7n8qNTmmhu!(Em;k)BICy#VuFzFeLn=! z71HNPaBJ+h(|zDkvTT14QSn2IUrrZ$Ui=W@Rr2gMKGwqU{#_RN&67JJxcVCb zLtak7H@Cfw38y0AGR2-gRO$JB6DY~PI$~02nHQ>fY4JgCo#fMk!AqB?Ps!wt8M0&0 z2Xg-wBOic#{TrW){)gO{1hEh+nYG}?uHp{whz7z#x7uOQJ7|DeT4_}rEJkZjy0<45I$-MrP)aX~ zkS;aoV5`@c4Gslv&#`*v#KT!Nnm0JM#(-LCD1wJVd0c#bUnC{Zg?ZlERM@5RX3m2~ zkY<%bi}z41Jgk5=uC+*=8DQ5nWo6aba|?3w>TY(sR!CpEbb?lK1rb--C=l@l$^t7IQCKvfNffp99a_-=$43N2Z8cRZ zdz9N~#_DRUv5p{r;;K~(YSL*<1Suwt5=FGyX;WhpOvXpF=lr`^OtEz`>7Duhd+xda zxsUIjbM7raE|)jTn^OXf8jVIvc=Y)3W2!Vj=zA!)x8$Ljc_Zy;Ixdzf;DmBlii5JA z)aOmm6MPAS2%`vW>qB(_p%=lAFqB{*gb`jK#1Og@q6j?*;e_6V0fg~{J_HX!1i?h$ zF$1aomTE3R8cmsIASt*JM|GrI-b!^MA=%ybqWVQbKSHou$2KV>hjeO(l03}aPNmvH zNF$^ZtOT~nbk}339;?9!*~8c0t>W%zOppWoU+$#IBRL=QZHYWabZwWnL7xZ zWs}`cXO{{+TC3mh#I8p_C>!p!V1~O9{u{$QJ2T96R}8bhD~5Gs!whqOXNHOKGF8b8 z^YL{sOtfpB#ju(Bx?-4*ohQhm`uQ2gP}%DVyZy;D_WQEyWzzkE?9uF12uJWOuSlK3 z&EsA%fxPEq)c76YIDy-bDrW}Jp`edsN2zrR&u`4a zGcToJ>qaY{SvyrR2DO4c-Kyt!;<4Efi_>j-4k~sDRw(W1K7i6YBa{(?JtZCui-+WR z0=oof64j`kG^(?3$hn3Yx<-GI8eHt-t$L0}j&)b(_y)}Q@hUu{jhBazay{N#k)`K6 zA-R8nC0O!CwhZk3E<)e1={T-cvL}#``T{OrnXTin;W0hg2T0avlyBp-lGoVTg@p?z zFI-$WOXQA&K>SI{^ninK{SP-=Vs9^-Q1QC{m8@+Sg(SXs4MIfxGzeDeUbTQDUa|)n z(O9uZ$MNf$x#DFVrwiB0S8u?!uM4nXVyOR{GxFB23cK{pr+BfZBsBZ6hP(~mo_K@v zFh({F*Ur(*$KOv%tBn~8I?he55&GKn?3)(1Larp?uySu`H?P_OM*!Z$qj8Zs-VL_I z#n?^z{c%Iu9lXcDt#uhwu(z zFM(ZQAEBl)H&Q-CLalPhn#|Vn&er|~>ay!cnBZ*$(?;Ah)nG6zxDvBy&sjsP_QJvu z&JRQG9$0&s5@)tojoB|ra_YQa}WvBU~p9q1lW@k z?U_sb`zE)9%^n;)u=(&u;q{m1c2lR?mf0MzXvJ5f{gy4iAO5H6*vY2bbjr0fd6|8P zRsWPJf8nlM1BEdcbokR&FJT)5{>p@WMf-o<-BgRpR~Z~YFsIsc zkKwUtnYG#Pfv*@p?lbM4Y2Wl8SQ>W#>yxIVan5c}&0tzR`?~4`?Q?AG{8&8Krb}Wa z1{TZK+ z{^jcJ>sx+3XVdo0w@zs^M-U21L{T7Eak(wCa$<;baft~W3!dpPKg7E1>1X~k@z`U} z==q(|m*8S9w!srPe)_mIasmn4@ZL%_(_wKb<)#=?^?PDbs-|KrE_0=1&O$!VmvAgR z250v!5{~zighT&4P8$6`95G=fZt!dKc)C*8^E4u)$^k& z%B{rWL|4v;?)a;{+ze1_2&sx?Q=MEJdzvni&h&b#^wJDe1SCMD60xNXYD+WJn~+G|AQE~*Bt}+U zh6;R8b^YII1lP@*j%VljD7Uut(mIYyxKzDFZh~Trq3f=5M&w}dRuiFnrd4r}fbj=x zSU$;u!@$MIu<6@!G*(Vfn{;+;lZb zsX1aJ#_zT&ttZDxG~Zi)pNow(*-G~9BT&2LzSL>+QOc2rUr5T`Fa5zjuaFb!Q%vB@ z_9P{_{R2>k!g2R~y!fE`_Omv)=QJq#2dE3=%gt5Hb^MRIF5Zc=Oa~WqLHz3OD7*)L zjaZv_3^g4(_P9CU*8;*a04$h)V8GLiwi#WF$}UEksQs5O;Jek$&matBWml_R4w}?4 zuO_yxf@F1Rf9{}66j{JW`R!ME5^XYH^5x5vi?so)BX+mcOSr1$VQ+Me$rvCJ4rG##qTFAflqjJ^&UvJ z>?R?d@ZFIQXF&w)HGm|FdO@}*3WiwRJgi)4IUgW7y9L8Nk18|hL~Jk=x%KpfYMZzN zXN=Lk9|||MxU>HZF{1~BIdy&Dp~qGWWby$;JF}DEBfx7ZMv;;MzRt#p5D%g*2L_44 zDPXC}gtN}G!yrlC8w3*8ewr=nf}tlGzAkXqnZcsPMODL;nhQ3`r8izkcU}pFOIl$~ zfCU`;qr{A%uybo8c+ire=X_CQgj1q93bu&EC~yc`PLZ(n0P}YiCTL+bq9zY2z`ivB z^3>Ie&3Ry?gBSI$K{#FeBgNGzFiUh#hJ11QRmc%FsnAQbCWCEnCiop51%+aw55_vfVjmCHCG|3wPr8GMYY`sqhSE(eth.received_packet))); - eth.new_data = false; + // Check to see if calibration requested + if (eth.command_data.status == slidersystem::SYSTEM_CALIBRATING) { + motor0.calibrate(eth); + } + else { + // Set the new target velocity + motor0.set_velocity(eth.command_data.vel_command); + eth.new_data = false; + } } // Limit switch check if (neg_lim_switch_flag) { motor0.target_velocity = 0; motor0.move_at_target_velocity(true); - //system_state = slidersystem::SystemStatus::NEG_LIM; + system_status = slidersystem::NEG_LIM; + neg_lim_switch_flag = false; } if (pos_lim_switch_flag) { motor0.target_velocity = 0; motor0.move_at_target_velocity(true); - //system_state = slidersystem::SystemStatus::POS_LIM; + system_status = slidersystem::POS_LIM; + pos_lim_switch_flag = false; + } + + // System status update (pins are logic high) + if (motor0.limit_switch_pin_neg.State() || motor0.limit_switch_pin_pos.State()) { + system_status = slidersystem::SYSTEM_OK; } // E stop check while (e_stop_flag) { motor0.target_velocity = 0; motor0.move_at_target_velocity(true); - //system_state = slidersystem::SystemStatus::E_STOP; + //system_status = slidersystem::SystemStatus::E_STOP; +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); - eth.send_packet(system_state, motor0.target_velocity); +#endif // __SERIAL_DEBUG__ + eth.send_packet(system_status, motor0.target_velocity); Delay_ms(1000); } @@ -160,9 +185,9 @@ int main(void) { //// Get current velocity //float motor_vel = motor0.get_velocity(); //ConnectorUsb.SendLine(motor_vel); - ConnectorUsb.SendLine(motor0.target_velocity); + // Send status, velocity data to the ROS2 node. - eth.send_packet(system_state, motor0.target_velocity); + eth.send_packet(system_status, motor0.target_velocity); } } \ No newline at end of file diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 6472223..7ac2832 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -5,6 +5,10 @@ * Author: Luke Strohbehn */ +#ifndef __SERIAL_DEBUG__ +#define __SERIAL_DEBUG__ 1 +#endif + #include "ClearPathMC.h" #include "interrupts.h" @@ -39,19 +43,19 @@ void ClearPathMC::begin() { // Enable the motor motor.EnableRequest(true); +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motor enabled."); - +#endif // Enable pin interrupts - limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::RISING, true); - limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::RISING, true); + limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::FALLING, true); + limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::FALLING, true); emergency_stop_pin.InterruptHandlerSet(&emergency_stop_callback, InputManager::RISING, true); - - // Wait for HLFB assert_HLFB(); - +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motor setup complete."); +#endif } @@ -61,11 +65,15 @@ bool ClearPathMC::check_for_faults() { */ if (motor.StatusReg().bit.MotorInFault) { if (HANDLE_MOTOR_FAULTS) { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motor fault detected. Move canceled."); +#endif handle_motor_faults(); } else { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motor fault detected. Move canceled. Enable automatic fault handling by setting HANDLE_MOTOR FAULTS to 1."); +#endif } return true; } @@ -78,7 +86,9 @@ void ClearPathMC::handle_motor_faults() { * Assumes motor is in fault * (this function is called when motor.StatusReg.MotorInFault == true) */ +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Handling fault: clearing faults by cycling enable signal to motor."); +#endif motor.EnableRequest(false); Delay_ms(10); motor.EnableRequest(true); @@ -89,6 +99,7 @@ void ClearPathMC::handle_motor_faults() { void ClearPathMC::assert_HLFB() { /* Make sure the HLFB is connected */ while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED && !motor.StatusReg().bit.MotorInFault) { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("ERROR IN HLFB ASSERT:"); ConnectorUsb.Send("\tHLFB STATE: "); ConnectorUsb.SendLine(motor.HlfbState()); @@ -97,6 +108,7 @@ void ClearPathMC::assert_HLFB() { ConnectorUsb.Send("\tHLFB Percent: "); ConnectorUsb.SendLine(motor.HlfbPercent()); +#endif Delay_ms(100); continue; } @@ -109,7 +121,7 @@ float ClearPathMC::get_velocity() { The duty cycle scales as a percentage of the maximum motor speed configured in the currently selected operating mode. - 5% duty cycle = 0% max speed - 95% duty cycle = 100% max speed - HLFB output deasserts (i.e., 0% duty cycle, "off", non-conducting) when the motor is disabled or shutdown. + HLFB output de-asserts (i.e., 0% duty cycle, "off", non-conducting) when the motor is disabled or shutdown. */ MotorDriver::HlfbStates hlfb_state = motor.HlfbState(); if (hlfb_state == MotorDriver::HLFB_HAS_MEASUREMENT) { @@ -123,18 +135,18 @@ float ClearPathMC::get_velocity() { } } -void ClearPathMC::set_velocity(double vel) { +void ClearPathMC::set_velocity(int vel) { /* Set the target velocity of the ClearPath MC motor, according to maximum velocity limits */ if (vel > max_velocity_CCW) { target_velocity = max_velocity_CW; } - else if (vel < -max_velocity_CCW) { + else if (vel < -1 * max_velocity_CCW) { target_velocity = max_velocity_CCW; } else { target_velocity = vel; } -} +} void ClearPathMC::move_at_target_velocity(bool hard_stop) { @@ -200,14 +212,60 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { // Wait for High-Level Feedback (HLFB) to assert (signaling if the motor has reached // its target velocity) +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Ramping speed, waiting for HLFB."); +#endif assert_HLFB(); // Check to see if motor faulted during move if (check_for_faults()) { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motion may not have completed as expected. Proceed with caution."); +#endif } else { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Move done."); +#endif } +} + +void ClearPathMC::calibrate(EthUDP& _eth) { + /* Send the moving base to the motor-side (negative) limit switch */ + while (system_status == slidersystem::SYSTEM_CALIBRATING) { + if (neg_lim_switch_flag) { + // If limit is reached, stop moving + target_velocity = 0; + move_at_target_velocity(true); + neg_lim_switch_flag = false; + system_status = slidersystem::NEG_LIM; + _eth.send_packet(system_status, target_velocity); + + ConnectorUsb.SendLine(system_status); + //ConnectorUsb.SendLine(system_status); + return; + } + + ConnectorUsb.SendLine(system_status); + + + //// If on the switch, move the slider just barely off the switch + //if (system_status == slidersystem::NEG_LIM) { + //if (limit_switch_pin_neg.State() == true) { + //target_velocity = 10; + //move_at_target_velocity(); + //} + //else { + //target_velocity = 0; + //move_at_target_velocity(true); + //system_status = slidersystem::SYSTEM_STANDBY; + //} + //_eth.send_packet(system_status, target_velocity); + //} + + // During calibration, move toward negative limit switch + target_velocity = calibration_velocity; + move_at_target_velocity(); + _eth.send_packet(system_status, target_velocity); + } } \ No newline at end of file diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 4d305f3..6178e28 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -5,52 +5,55 @@ * Author: Luke Strohbehn */ +#ifndef __SERIAL_DEBUG__ +#define __SERIAL_DEBUG__ 1 +#endif #include "EthUDP.h" EthUDP::EthUDP(): /* Initialize class variables */ - local_ip(169, 254, 97, 177), - local_port {8888}, - remote_ip(169, 254, 57, 209), - remote_port {8888} + m_local_ip(169, 254, 97, 177), + m_local_port {8888}, + m_remote_ip(169, 254, 57, 209), + m_remote_port {8888} { } EthUDP::EthUDP(IpAddress _local_ip): - local_ip(_local_ip), - local_port {8888}, - remote_ip(169, 254, 57, 209), - remote_port {8888} + m_local_ip(_local_ip), + m_local_port {8888}, + m_remote_ip(169, 254, 57, 209), + m_remote_port {8888} { } EthUDP::EthUDP(IpAddress _local_ip, int _local_port): - local_ip(_local_ip), - local_port(_local_port), - remote_ip(169, 254, 57, 209), - remote_port {8888} + m_local_ip(_local_ip), + m_local_port(_local_port), + m_remote_ip(169, 254, 57, 209), + m_remote_port {8888} { } EthUDP::EthUDP(IpAddress _local_ip, IpAddress _remote_ip): - local_ip(_local_ip), - local_port {8888}, - remote_ip(_remote_ip), - remote_port {8888} + m_local_ip(_local_ip), + m_local_port {8888}, + m_remote_ip(_remote_ip), + m_remote_port {8888} { } EthUDP::EthUDP(IpAddress _local_ip, int _local_port, IpAddress _remote_ip, int _remote_port): - local_ip(_local_ip), - local_port(_local_port), - remote_ip(_remote_ip), - remote_port(_remote_port) + m_local_ip(_local_ip), + m_local_port(_local_port), + m_remote_ip(_remote_ip), + m_remote_port(_remote_port) { } @@ -60,35 +63,41 @@ EthUDP::~EthUDP() {} void EthUDP::begin(void) { /* Set up UDP Ethernet communication */ // Check physical Ethernet link + while (!EthernetMgr.PhyLinkActive()) { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Could not detect a physical Ethernet connection."); +#endif Delay_ms(1000); } // Run the setup for the ClearCore Ethernet manager EthernetMgr.Setup(); - if (using_dhcp) { + if (m_using_dhcp) { bool dhcp_success = EthernetMgr.DhcpBegin(); if (dhcp_success) { +#if __SERIAL_DEBUG__ ConnectorUsb.Send("DHCP successfully assigned an IP address: "); ConnectorUsb.SendLine(EthernetMgr.LocalIp().StringValue()); +#endif } else { - while (1) { + while (true) { +#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("DHCP configuration was unsuccessful."); +#endif Delay_ms(10000); - continue; } } } else { - EthernetMgr.LocalIp(local_ip); + EthernetMgr.LocalIp(m_local_ip); //EthernetMgr.GatewayIp(IpAddress(169,254, 93, 234)); //EthernetMgr.NetmaskIp(IpAddress(255, 255, 0, 0)); } // Begin listening on the local port for UDP datagrams - udp.Begin(local_port); + udp.Begin(m_local_port); } @@ -98,28 +107,51 @@ void EthUDP::read_packet(void) { if (packet_size > 0) { uint32_t bytes_read = udp.PacketRead(received_packet, MAX_PACKET_LENGTH); new_data = true; + + // Parse data from the received packet + // Extract first field + char* received_packet_cstr = reinterpret_cast(received_packet); + token = strtok(received_packet_cstr, delimiter); + + if (token != NULL) { + command_data.status = static_cast(atoi(token)); +//#if __SERIAL_DEBUG__ +//ConnectorUsb.Send("Field 1 is:"); +//ConnectorUsb.SendLine(command_data.status); +//#endif + + // Extract second field + token = strtok(NULL, delimiter); + //token_cstr = reinterpret_cast(token); + if (token != NULL) { + command_data.vel_command = atof(token); +//#if __SERIAL_DEBUG__ +//ConnectorUsb.Send("Field 2 is:"); +//ConnectorUsb.SendLine(command_data.vel_command); +//#endif + } + } } } char* EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float data) { /* Construct the message to send to the ROS2 Node on the host computer - https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ - + // Reset buffers memset(&msg_buf[0], 0, sizeof(msg_buf)); - char status_buf[2]; + memset(&status_buf[0], 0, sizeof(status_buf)); + memset(&data_buf[0], 0, sizeof(data_buf)); + + // Set data sprintf(status_buf, "%d", system_status); - char data_buf[10]; sprintf(data_buf, "%f", data); - char status_header[11] = "{\"status\":"; - char vel_header[13] = "\"servo_rpm\":"; - char footer[2] = "}"; + // Create c-str msg strcpy(msg_buf, status_header); strcat(msg_buf, status_buf); strcat(msg_buf, ","); - strcat(msg_buf, vel_header); + strcat(msg_buf, data_header); strcat(msg_buf, data_buf); strcat(msg_buf, footer); return msg_buf; @@ -130,9 +162,7 @@ void EthUDP::send_packet(slidersystem::SystemStatus system_status, float data) { /* Send a packet */ char* msg = construct_data_msg(system_status, data); - udp.Connect(remote_ip, remote_port); + udp.Connect(m_remote_ip, m_remote_port); udp.PacketWrite(msg); udp.PacketSend(); -} - - +} \ No newline at end of file From 7e870727b7ce913f75de0adfce1682e78ed2227b Mon Sep 17 00:00:00 2001 From: lukestroh Date: Tue, 2 Apr 2024 22:41:09 -0700 Subject: [PATCH 04/21] pointers and such. --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 89600 -> 89600 bytes include/EthUDP.h | 6 +-- include/clearpathmc.h | 8 ++-- main.cpp | 51 +++++++-------------- src/ClearPathMC.cpp | 42 ++++++----------- src/EthUDP.cpp | 9 ++-- 6 files changed, 41 insertions(+), 75 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 98f9d58ea3d69b0cbc1197176fe240841f705690..cc371949ce643627eb6017020ed623039b70ec81 100644 GIT binary patch delta 283 zcmZqJ!`iTib%FsS(?mmQM&^ya@@%Xd-2d{_Z7yWn&gjC&z`(!@#QZ?)48;Hb|Nn0U zq}hQO$oww>q~(B^1&C#!Vk$t|4v00N{K+{iGAtm)lY3ZHR84`RTtKW2l{E&^0zm8p z#A-k+4#ZO@2Fg$V!Xm<`JDHQ!o6%r%DCcRO$qHr~ERI0g$qf=3lS|A3CLduknEb^o zfWrYO2yz@m!n|klqf369{UkOyZ&EqI$O1ALB*QZi$k+zNAh0NoWsx4!Br~PSDq=F5 c?NSagPk!_*ZgZU5BF05oOp9We7Ug{a0P@~YwEzGB delta 358 zcmZqJ!`iTib%Md<4Sk#wErl5wH~Px6v3^)omu|PYkZn7o13v=;11}K&2Lfjx{SQbR z0cmz1mI7i4AeIASkh~0#Rsv!bAhv^wP0nDkPyk8l00p#xSRROVfp{tqs{=7eZt?*Z z9Z?o2M*&DXK-pZA8CjhLgQ2_-AT12UVw*!bPxDMxkXpf_2jos}kkFX?MarL(5oDG= zOr%8GfAV1_!^vNyqc;0VY;p!F&(f>4U|-bD&RH)GCF}62sZyuW@MSHq|q^X+N{vccV{UxF3Mt>BnQ-C0hDzG UVvvhAm&oxjPHt#zSd{kx0LxZwP5=M^ diff --git a/include/EthUDP.h b/include/EthUDP.h index 725d774..d39390a 100644 --- a/include/EthUDP.h +++ b/include/EthUDP.h @@ -35,7 +35,7 @@ class EthUDP { char data_buf[10]; const char status_header[11] = "{\"status\":"; const char data_header[13] = "\"servo_rpm\":"; - const char footer[2] = "}"; + const char footer[2] = "}"; public: // Data buffer @@ -43,7 +43,7 @@ class EthUDP { bool m_using_dhcp = false; const uint8_t MAX_PACKET_LENGTH = 128; // Maximum number of characters to receive from an incoming packet unsigned char received_packet[128]; // Buffer for holding received packets - char msg_buf[128]; + char msg_buf[128]; // Send message buffer // Ethernet UDP EthernetUdp udp; @@ -61,7 +61,7 @@ class EthUDP { // Public methods void begin(); void read_packet(); - char* construct_data_msg(slidersystem::SystemStatus system_status, float data); + void construct_data_msg(slidersystem::SystemStatus system_status, float data); void send_packet(slidersystem::SystemStatus system_status, float data); }; diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 4dbed63..120d6e8 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -31,9 +31,9 @@ class ClearPathMC { // A reference to the maximum clockwise and counter-clockwise velocities set in // the MSP software. These must match the values in MSP software - const int32_t max_velocity_CW = 1000; - const int32_t max_velocity_CCW = 1000; - const int8_t calibration_velocity = 100; + const int32_t m_max_velocity_CW = 1000; + const int32_t m_max_velocity_CCW = -1000; + const int8_t m_calibration_velocity = -100; // Each velocity commanded will be a multiple of this value, which must match // the Velocity Resolution value in MSP. Use a lower value here (and in MSP) to @@ -63,7 +63,7 @@ class ClearPathMC { void set_velocity(int vel); void move_at_target_velocity(bool hard_stop = false); - void calibrate(EthUDP& _eth); + void calibrate(EthUDP* _eth); //void stop(); }; diff --git a/main.cpp b/main.cpp index b381e43..f49ef3c 100644 --- a/main.cpp +++ b/main.cpp @@ -87,30 +87,16 @@ void set_up_serial(void) { } } -//void reset_slider(void) { - ///* Reset the linear slider to position 0, where base is closest to the motor. - //Run the motor at a low constant velocity until the negative limit switch is trigged - //and send a message via Ethernet to ROS2. */ - //system_status = slidersystem::SYSTEM_CALIBRATING; - //motor0.target_velocity = -30; - //while (!neg_lim_switch_flag) { - //motor0.move_at_target_velocity(); - //eth.send_packet(system_status, motor0.target_velocity); - //} - //motor0.target_velocity = 0; - //motor0.move_at_target_velocity(true); - //system_status = slidersystem::SYSTEM_STANDBY; -//} - -bool read_switch(DigitalIn& switch_pin, bool& interrupt_flag) { - if (interrupt_flag) { +bool read_switch(DigitalIn& switch_pin, volatile bool* interrupt_flag) { + /* Returns true if the interrupt has been triggered */ + if (*interrupt_flag) { bool reading = switch_pin.State(); static bool change_pending = false; if (!reading) { change_pending = true; } if (reading && change_pending) { - interrupt_flag = false; + *interrupt_flag = false; change_pending = false; return true; } @@ -124,13 +110,12 @@ int main(void) { set_up_serial(); #endif - eth.begin(); motor0.begin(); system_status = slidersystem::SYSTEM_CALIBRATING; - motor0.calibrate(eth); + //motor0.calibrate(eth); - // Main run loop + // Main loop while (true) { // Read data from the ROS2 hardware interface. eth.read_packet(); @@ -139,7 +124,7 @@ int main(void) { if (eth.new_data) { // Check to see if calibration requested if (eth.command_data.status == slidersystem::SYSTEM_CALIBRATING) { - motor0.calibrate(eth); + motor0.calibrate(ð); } else { // Set the new target velocity @@ -149,34 +134,32 @@ int main(void) { } // Limit switch check - if (neg_lim_switch_flag) { - motor0.target_velocity = 0; + if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { + motor0.set_velocity(0); motor0.move_at_target_velocity(true); system_status = slidersystem::NEG_LIM; - neg_lim_switch_flag = false; } - if (pos_lim_switch_flag) { - motor0.target_velocity = 0; + if (read_switch(motor0.limit_switch_pin_pos, &pos_lim_switch_flag)) { + motor0.set_velocity(0); motor0.move_at_target_velocity(true); system_status = slidersystem::POS_LIM; - pos_lim_switch_flag = false; } // System status update (pins are logic high) - if (motor0.limit_switch_pin_neg.State() || motor0.limit_switch_pin_pos.State()) { - system_status = slidersystem::SYSTEM_OK; - } + //if (read_switch(motor0.limit_switch_pin_neg) || read_switch(motor0.limit_switch_pin_pos)) { + //system_status = slidersystem::SYSTEM_OK; + //} // E stop check while (e_stop_flag) { - motor0.target_velocity = 0; + motor0.set_velocity(0); motor0.move_at_target_velocity(true); - //system_status = slidersystem::SystemStatus::E_STOP; + system_status = slidersystem::E_STOP; #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif // __SERIAL_DEBUG__ eth.send_packet(system_status, motor0.target_velocity); - Delay_ms(1000); + Delay_ms(5000); } // Move to target velocity (blocking) diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 7ac2832..9d50e8d 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -127,7 +127,7 @@ float ClearPathMC::get_velocity() { if (hlfb_state == MotorDriver::HLFB_HAS_MEASUREMENT) { // Get the measured speed as a percent of Max Speed float hlfb_vel_percent = motor.HlfbPercent(); - float hlfb_vel = hlfb_vel_percent * max_velocity_CW; + float hlfb_vel = hlfb_vel_percent * m_max_velocity_CW; return hlfb_vel; } else { @@ -137,11 +137,11 @@ float ClearPathMC::get_velocity() { void ClearPathMC::set_velocity(int vel) { /* Set the target velocity of the ClearPath MC motor, according to maximum velocity limits */ - if (vel > max_velocity_CCW) { - target_velocity = max_velocity_CW; + if (vel > m_max_velocity_CCW) { + target_velocity = m_max_velocity_CW; } - else if (vel < -1 * max_velocity_CCW) { - target_velocity = max_velocity_CCW; + else if (vel < m_max_velocity_CCW) { + target_velocity = m_max_velocity_CCW; } else { target_velocity = vel; @@ -221,25 +221,24 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { if (check_for_faults()) { #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motion may not have completed as expected. Proceed with caution."); -#endif } else { -#if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Move done."); #endif - } + } + } -void ClearPathMC::calibrate(EthUDP& _eth) { +void ClearPathMC::calibrate(EthUDP* _eth) { /* Send the moving base to the motor-side (negative) limit switch */ while (system_status == slidersystem::SYSTEM_CALIBRATING) { if (neg_lim_switch_flag) { // If limit is reached, stop moving - target_velocity = 0; + set_velocity(0); move_at_target_velocity(true); neg_lim_switch_flag = false; system_status = slidersystem::NEG_LIM; - _eth.send_packet(system_status, target_velocity); + _eth->send_packet(system_status, target_velocity); ConnectorUsb.SendLine(system_status); //ConnectorUsb.SendLine(system_status); @@ -248,24 +247,9 @@ void ClearPathMC::calibrate(EthUDP& _eth) { ConnectorUsb.SendLine(system_status); - - //// If on the switch, move the slider just barely off the switch - //if (system_status == slidersystem::NEG_LIM) { - //if (limit_switch_pin_neg.State() == true) { - //target_velocity = 10; - //move_at_target_velocity(); - //} - //else { - //target_velocity = 0; - //move_at_target_velocity(true); - //system_status = slidersystem::SYSTEM_STANDBY; - //} - //_eth.send_packet(system_status, target_velocity); - //} - // During calibration, move toward negative limit switch - target_velocity = calibration_velocity; + set_velocity(m_calibration_velocity); move_at_target_velocity(); - _eth.send_packet(system_status, target_velocity); + _eth->send_packet(system_status, target_velocity); } -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 6178e28..d3e9595 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -134,11 +134,12 @@ void EthUDP::read_packet(void) { } } -char* EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float data) { +void EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float data) { /* Construct the message to send to the ROS2 Node on the host computer https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ // Reset buffers + ConnectorUsb.SendLine(status_header); memset(&msg_buf[0], 0, sizeof(msg_buf)); memset(&status_buf[0], 0, sizeof(status_buf)); memset(&data_buf[0], 0, sizeof(data_buf)); @@ -154,15 +155,13 @@ char* EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float strcat(msg_buf, data_header); strcat(msg_buf, data_buf); strcat(msg_buf, footer); - return msg_buf; } void EthUDP::send_packet(slidersystem::SystemStatus system_status, float data) { /* Send a packet */ - char* msg = construct_data_msg(system_status, data); - + construct_data_msg(system_status, data); udp.Connect(m_remote_ip, m_remote_port); - udp.PacketWrite(msg); + udp.PacketWrite(msg_buf); udp.PacketSend(); } \ No newline at end of file From 9a203d0550e51e1415adde56003fe3146ae64757 Mon Sep 17 00:00:00 2001 From: lukestroh <66746219+lukestroh@users.noreply.github.com> Date: Tue, 2 Apr 2024 22:56:57 -0700 Subject: [PATCH 05/21] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1f3f3fe..3bd2548 100644 --- a/README.md +++ b/README.md @@ -37,7 +37,7 @@ Velocity Control, then hit the OK button). Frequency" dropdown, and hit the OK button). ### LIMIT SWITCHES: -1. Limit switches should be connected to the I0 and I1 inputs on the controller. +1. Negative and Positive Limit switches should be connected to the DI-7 and DI-8 inputs on the controller, respectively. ### EMERGENCY STOP: @@ -49,4 +49,4 @@ ClearCore Manual: https://www.teknic.com/files/downloads/clearcore_user_manual.p Copyright (c) 2020 Teknic Inc. This work is free to use, copy and distribute under the terms of -the standard MIT permissive software license which can be found at https://opensource.org/licenses/MIT \ No newline at end of file +the standard MIT permissive software license which can be found at https://opensource.org/licenses/MIT From f0839e6e5bccb0706219e43e7e2e8bf038456f88 Mon Sep 17 00:00:00 2001 From: lukestroh Date: Wed, 3 Apr 2024 17:43:34 -0700 Subject: [PATCH 06/21] 'fixed' the bug by replacing with string literal. Unsatisfactory fix --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 89600 -> 86528 bytes include/EthUDP.h | 12 ++-- include/clearpathmc.h | 9 +-- include/interrupts.h | 4 -- include/system.h | 2 +- main.cpp | 73 +++++++++++---------- src/ClearPathMC.cpp | 26 ++++---- src/EthUDP.cpp | 29 +++----- 8 files changed, 70 insertions(+), 85 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index cc371949ce643627eb6017020ed623039b70ec81..3b8e91cf4d571f69444fec7aecdc88d66151e512 100644 GIT binary patch delta 3004 zcmcJReNa@_6~Nzl`xbUZ2w(1Q1eawYNQJ;I5h2#t4Q#fE$}Se2gpNwB?W87JG+Se+ zAM0W<9|mDh;8|)?vq7a9jSDw*JGNsnWI9b!A!yo}rWu8%t$&zgk`A$Dh;Gl>U8r?3 zor!7h%x})U=YE`f?>Xnb=lhk`*Q@QYVW?eiN|mHiNs=_gjYuRyLIUEh{`zcL8~$}9 zzYVv^7jy>F6%yz(72^&~-yUux5h+A6L31NZNb(|OBo`Ap;%=gZm`h|6cM$o+oy0u^ z*_Y-KS;PWjEs;kQ5VHt2%xlged7Wei$zn+=lQOA7?MM;H93q|QBIXkb1ox$ptRl>W zv9mtIZl%h-1b2QdF1dbxy#4^m+?({vJL`?PbvG&8RQC|8;<|5;Gx5-zS=ht;9CsF~Uo1C)niUBzF)`5I>CTc9Psh@PYS|#Lu8P!FH;s)%uw+lLM{e~*BI(MB+wsI#Xx?n%R!@+*GPW6EPq15V_5|x~q*DlTmo(!EJg+nU zpWV>#zjgy3W?)uz0uIS`Er-u>d?K}$Td*5HO}&V_=M>{an)|=?i-`=k%mdJCaO=TA zt8ipeJ{97GAvzVkMzV#xaYcf@<(fQ}8Rj%!lm8vXQhC5h`r8aXicw+csh>_X?p2bWH3yjjt6CUM3M z6I9fGBjjP~wSD54tr1W@x>zjkTPmluO#CTieKzYiPu_d6X4!W(F6J|d!>cwzU`4e) z`bP0lW1Cvx#_ARK1y$vpeQDZDROuy-632++qTlI;&Sq1IT7_Z%arfBn2#&e-z(;v^$MGuf~_n`Gc3$}P%X3IUTY%OYv%`K(J zJV%zK^W;CLUR?56A+XbluUFY#^6rF$M`&n!F1GBd)$wz}k2&9P(u|X-J$`LHu5+0_ zcTb|n^0>4KqaW$5_UU~rurTp)juV+J=W zfx?&5UZ}}=-u}yz^By~Vuq`rbOo@ioaA6q?Y-*QpV@~V5EH&)5EgSEa4sahMrwu% zj$bHJ9RQV>Qj(7^c&vD2o=FV0IJCj1ZtbnZ< zm&--y7al;%r(kp4w~#le(b@9#qGykRRTe4*LN8#l5teK^@HnRV~YfpsM6Sv+7BQF|Hj} zZ5ePTd?XK=61p7VV!vC}x^f7qq1E7rK$jg>$CMqb!2*)nDZ`cSN_Z>$mJ7-g5s<#c zCkn#81>nm%RtL0QH_h>cv!#O1|4(M3k WrVcg2!+7_7OVr`DV8-0a%zpvcvsRAvxxiX)C#v9(I4Fesn`V{q3W%8#X0%(m;|Kqza< zXw-I-4RPkQyvCZ=PP8!*Md!9oV_J*WHo9A}(xz=}bV1#wv8IyB8l|hV=e!xxuG+t( zH~HQ@_uPBWz4x4Z-g};1isv0=o82PmA|*kmOD0iI+zy38tYz79(3K%6Uv{o8+JY_8 zKy*CmtOR<}Y}l^f(>aeCaRki?ap!$R3Bg7RsE#Hk5NX6D!a!sYSwuEbM6khe#QnrX zB9B-~OeS)OTVyMjYPOj|^>`wmctEEs&?S*zBc>9W#B_qjg{DybQbeZ1>53zAs@FBS z(@YIVL}857iBvm@`H}Vls>_ISqJmgNu;Im2yCO2TxuoCGHc|VJ#4=(zv4U7h)DbjX z(I#PgV@3*}X*Frr5Y5C|qSfn~3LRco5_Hl-_K?aa{mF1(!7bx+_X}2bA4MiZf#DMSXXDAPo<-Pc9LWKf+Dts=*22bIC5tj)aU`+o zR}K{;NmWt|u$vwmo9#ByY-t9Ln5tsHL0V2M&8|CD>k%WVHVMu7dt|=#;T7}4bKKLb z6yPM`ZN;^d&`(gop@Di)m+@~9CGA^4=9J-y$}tj-iJO^GQXsnwWX{>}QaROlTyKwi zg%npf*>pM)Dme-A*ftd}sSb&omVZEumgbzcJk>CRgzY*UU40;N_-x#N7k!b_d+*!*O{xt>QZ;<>p$zLp~-^D?bXtM7BUz-z;OLla(RY#l21ivc6XKOpn-i+h- zLk&R%VfQ0qPQi!ZbDGg?n#}!3ev8r4yO>94abOR!Z}0S=@+uAaTHKr+73<4$M)RfP zGlox|SB=gkb{xqgmuAgF&(spG;RUZN6JGSXrW;in+D$yjQ)*+=64}~o#7^RMVz1XV zC8mdjeZ=j9jSIn7*_PO4XxhE?fWea1GokkQx%^(u8QW`~0)Lt8E2KpVX)&&|=J{rq zVOfq1-LvUPoZJ?IcV{dl;co<&fb~s?|0^tROKu!o{Px>d(_g@ixsPJjKnHeI&!2Sp z`uR~F#tz`Jg2PN4#fHDEWiub>qOmQKLl2M3yB|`ICpKsS>>NY3!eYi%+87x z6!E1XdzXyI@wv~y4zV$}69Uc?AdflNsNDfxgNnaS^3`@>|Kkbf(v24$xz_1?r={?# z%kbS3UECesL->hDYMp3)u?QWH_XUd0k}x+7>Ul3%SDE(HehRGZl76%O-*T-)X!+H9 zxB-W**Z;!KI1;5(I{uti|Jn-o$-}q)FA~bTyPIk_+UigxSp6=({dnxQ6NmRDsOK{v zR&B@x8OIf8B!}DbyGUng}vuvh%2QxJm*=I?i3E~e;Ie)7>7#_+=NfGmgiKoy3PSc z+%#+zgZqml!}@2}yPtd-J;Qm}e)hBbK&JyjE6t7{o3KJu;G9n}@#i#LEPb^k@H@%* zDO(qgi$NLljv8b6S0a)1r%AuWe?>y?smdRgnmQ&P9CqC;ipMS;(rae;k4H0e_vYh` z&Cj@>Y-wp-=U$(`MzuI#rS=ddm)3&+x+DgVR7mj(83-3kpIMx_>QpqloavOLIwzm6 zY8TxLdvAHfQ&xz@svGrU`HfcD#6Krvg?}N~GtL1%t72+7Ud z%EfJt(O`5eSPgI4;bVQb2{x$3>99`SnF_Yx);#EdLlfXDSzV=vRqCEp$kvn-V|ZEq z(*HrVOo2tgrCG2yO0Ap&26dtn<^~l9tk$dFmcTUiy=l;ZaSs)$x2qu`xS;|z18&>5 zPD`0!!z?%q!N1Ihk3fC>0Z2mI%4GHOLMT;Rv*B}mdTzaHT?|JxBWj)t)&w*2VF=K% Ta4SxmH&^X m_max_velocity_CCW) { + if (vel > m_max_velocity_CW) { target_velocity = m_max_velocity_CW; } else if (vel < m_max_velocity_CCW) { @@ -146,7 +149,7 @@ void ClearPathMC::set_velocity(int vel) { else { target_velocity = vel; } -} +} void ClearPathMC::move_at_target_velocity(bool hard_stop) { @@ -155,6 +158,7 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { // Check motor status check_for_faults(); + // TODO: Handle this limit elsewhere // If at negative limit switch, don't let target velocity be negative if (neg_lim_switch_flag && target_velocity < 0) { target_velocity = 0; @@ -229,7 +233,7 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { } -void ClearPathMC::calibrate(EthUDP* _eth) { +void ClearPathMC::calibrate() { /* Send the moving base to the motor-side (negative) limit switch */ while (system_status == slidersystem::SYSTEM_CALIBRATING) { if (neg_lim_switch_flag) { @@ -238,18 +242,16 @@ void ClearPathMC::calibrate(EthUDP* _eth) { move_at_target_velocity(true); neg_lim_switch_flag = false; system_status = slidersystem::NEG_LIM; - _eth->send_packet(system_status, target_velocity); + //_eth.send_packet(&system_status, target_velocity); - ConnectorUsb.SendLine(system_status); //ConnectorUsb.SendLine(system_status); return; } - ConnectorUsb.SendLine(system_status); // During calibration, move toward negative limit switch set_velocity(m_calibration_velocity); move_at_target_velocity(); - _eth->send_packet(system_status, target_velocity); + //_eth.send_packet(&system_status, target_velocity); } -} \ No newline at end of file +} \ No newline at end of file diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index d3e9595..8cfd2bb 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -111,30 +111,21 @@ void EthUDP::read_packet(void) { // Parse data from the received packet // Extract first field char* received_packet_cstr = reinterpret_cast(received_packet); - token = strtok(received_packet_cstr, delimiter); + m_token = strtok(received_packet_cstr, m_delimiter); - if (token != NULL) { - command_data.status = static_cast(atoi(token)); -//#if __SERIAL_DEBUG__ -//ConnectorUsb.Send("Field 1 is:"); -//ConnectorUsb.SendLine(command_data.status); -//#endif - + if (m_token != NULL) { + command_data.status = static_cast(atoi(m_token)); // Extract second field - token = strtok(NULL, delimiter); + m_token = strtok(NULL, m_delimiter); //token_cstr = reinterpret_cast(token); - if (token != NULL) { - command_data.vel_command = atof(token); -//#if __SERIAL_DEBUG__ -//ConnectorUsb.Send("Field 2 is:"); -//ConnectorUsb.SendLine(command_data.vel_command); -//#endif + if (m_token != NULL) { + command_data.vel_command = atof(m_token); } } } } -void EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float data) { +void EthUDP::construct_data_msg(slidersystem::SystemStatus* system_status, float data) { /* Construct the message to send to the ROS2 Node on the host computer https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ @@ -145,11 +136,11 @@ void EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float memset(&data_buf[0], 0, sizeof(data_buf)); // Set data - sprintf(status_buf, "%d", system_status); + sprintf(status_buf, "%d", *system_status); sprintf(data_buf, "%f", data); // Create c-str msg - strcpy(msg_buf, status_header); + strcat(msg_buf, "{\"status\":"); // TODO: For some reason status_header gets set to 0. Needs a debugger. strcat(msg_buf, status_buf); strcat(msg_buf, ","); strcat(msg_buf, data_header); @@ -158,7 +149,7 @@ void EthUDP::construct_data_msg(slidersystem::SystemStatus system_status, float } -void EthUDP::send_packet(slidersystem::SystemStatus system_status, float data) { +void EthUDP::send_packet(slidersystem::SystemStatus* system_status, float data) { /* Send a packet */ construct_data_msg(system_status, data); udp.Connect(m_remote_ip, m_remote_port); From e0b70e9b31a06c360195f146b2c101e3752fe090 Mon Sep 17 00:00:00 2001 From: lukestroh Date: Tue, 16 Jul 2024 21:58:24 -0700 Subject: [PATCH 07/21] updated things, still have extern system_status, next commit will change this --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 86528 -> 81408 bytes include/EthUDP.h | 15 +-- include/clearpathmc.h | 16 ++- include/interrupts.h | 2 +- include/system.h | 14 +++ main.cpp | 107 ++++++++++++++------- src/ClearPathMC.cpp | 94 +++++++++++++----- src/EthUDP.cpp | 13 ++- 8 files changed, 174 insertions(+), 87 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 3b8e91cf4d571f69444fec7aecdc88d66151e512..95f9ff4238f8c7f6bf20c765179e7c53bcbc2438 100644 GIT binary patch delta 4575 zcmd5<3sjRw7XI&_2Z5jjh#?3{2nvV@0VH~;k0?P!F(y{6b*UO(pamZgEyV|cZC$%6 znq2E&6 zX8t>O?#z7m&Yg+#ZJG0~bhRO#=K@8=acXKx2raFxtxU?=@w;s?ynOYy-M@WpJ(lq{ zJS@q8uf!XCPpC?yekGwZp#vd|FoH0ckVyz3L=e&lWS)}~VhIL9H$ra$P1Tx5br(Vm zAtfM7pgNrJZ|YB_nysl5)jC2tfj%=^yHi6?=t+npXbHPhXm2f;*$B+!7?D3>j+ z`+&elDAgSaiokcjK+P0eT=pnTmFYT+A>mlUIKoRVTMSHf*`nZ8mn{kR$Wmk3s(J;+ zEuwlcVF{s{fOlc8eD#iWKXEmTGc?TKT0Z0cx|wR@$0mo4EbNYg%v zG`tzyJ9GsZTS;gnVBe6t`AyU~N%)L#ig226g>Z(zES;tL&xCV?zXW7oP!ezQ_L`?c?IGJN#-HR;nuH3u)5TI=ro_gatT1yeeci30R=frdq-(pQHw}#T$_p zo}DptXhsG5e88NFPHi$alLQ#ixor|h)F?d6XDBz3fbE|OUOqiav}jKQ>e9AJ*-7z_ zP4j7@BYl(%*m=Yyol=z*)L{Hlp>dPMiLoAV>vR3{>uH3szdPZGf*mUsR(?wfsiIDI z3F^&%0_mWDROzC|J^~fI{3BVis31~tNclr@MbV)xiQ9g!4XUtkTs<~sRn;f&fer`d zdzaaBdAGGeR(ZU~$DGR+snrH^+$x|=QYwn${b5RDTE**D6$t-Q!ZdN3p$^h} zQVJ>LjFhRe2pQBaNc4?ckCo$8)jwH|uEfz?G3kVZV zbz#b7+;kc?jl1wkOYz?ZSDnlJL^7sN#}c`3fUN-3!uJUfArx3as|r;bl^r*fj(zRr z_|W}9NBao&95A3WPLKM$c=BpC9}8F+myDh~y@F*$<^|>f`RCN&{eAi(y@KtBWnEw2 z`TKny4KtIWTu!1x1S;6tC11+2Ez=bp6;@D~>1$X(B4cVz;;lYG@`s|plK>u*)LqaQ zC^JU(SVMYm64nv~!drxOg!P0CggU}TLd$OZD!^TnB1BDL3Pk@1Q@I@Cb}kV$hby9A zJMpaJW}xf!s1A(pQX;T~G|Dml(_eaqN!>e~;a5KX=0f7NS4Y;otax$6`f~!0t{v%6 zN0GD=n_g1skL`EO$_PH;Sh8qpOnvCtEi|t$9Wh?~1)V#c?$Hh?#rdNxGEkEO3x8}L zZB?)g$L=B@d!^Lar^d?X^MidRSk7e~=;)b>I%7gG3?~V@Y5YOJM%vM?t>p9l2|xl^$6|Sp%ChezJu=K--Xxh-JX_s;LgYot~M{W%$Pfm zX^Bn6nc$hoyHm`Hd|D*CFxW!DhVxUZz$)HIQ9}KMYoLnuzCI|ufA)c4`y&Tj$=kkF zY?;bSg_>xJNy857PdcF}Ji>z!8in(-%&oznPd`TUJjMy0Sr1e5%$&T30^VKzXxc;F zkYs6_tI<(&MmjV=Cpw0%K>g`Tth5d>Ok6jP-H7+L{{=mJ!YKFr*Ie}M@UXy)M|cA| z7FZQJ+J&Cr3-;y(Cxd~7zVG-@49Qqh8iTjWhX?x}A?z{cKNgO_J;h!U{;`geEEUPv z@KC(9aG~!gp7e#}y~FncbvDxBTWpcGojV!J>m1(78zp6LLBI7O5AyCd(t`E<&}qzl z%weqW<^cFL&hK6&N3!r$C-HW!e_SfQmu??S#dt(wkUyA#gSqw@L*~oHXv-h^TT`tx^h1gN{Y)2>0_|=! zy+{=W+$&r@XQj>7qE1|ir4sS>=I=nLS3~EYT=pQqL2CwLLtR7t1}BKlcb(vI^0X(D zXsT_Hc((Iyx9KUTg&MrG_R9S{i*CEGe-NZWP*=@uAf|$>+xkb|zuQZd`y;tZR;43V6nVilem-k*-hq)`RUu$l7W8JPb zqfH!-OHLT@=!xgB_2^B_uY3d8PjUmhlOgm;H*ID%ew1Z~RD`B#xQH38HlaWU&an0m3;MewRy9=z?+XQkH2Nm@bFRlX zdpzCcLH~jTS7&fxZDA~}GIaqx=OvK1PXh)J43RM4DIC&L4Z`FYuy`#} zXp#!2JHvS4kCBigG^k;Uuq_I@d6%X@CG6`AvjJ8A9D-k)-tj6zU_p?urZdzsrYY00 z|5HEHrM;forDQu1K|NETH74Tf)V1EX)o`2_4#mPWv~*H?zlnimfc5SiFD*DNsC@*s zf2q=cXW>*lq~m6Hf!7iTCjdPY$9wyB1?@BRDmF|!F3^%TJ>4#{*)De1clR#u2O}Vu zg>@!jwh*5L$wKXOU=%i6AVgYfcN7bQav&1Mv91E|SUt>>!ozxUM{(J7`!u221|xB1 z|E_{A31$kv83jXy!2==AJE=eLys*3n%oi*P2)TWL&>A%D%hI6j6BBV zVDebKk?ME?G0fdZ9z8R1S)cy$KXd6AUz6wc6NLKy7@_#RTbu`DwjY?}es?5$=r8Rw z{b|DG8cT*$rP?8T^zK+S;;O7(iRJ2Yb;a zsFH4YXUp)gG8W6UIc{L00W9^<>TAGmZH}|WCr1G+*YLHGQL1;yxk8|V@3u!`*Qrn` z+xIk}PCM%VoG4Dp53qrt)uTQkjGw|*U~!ZsIOFI}pa@y6PaSAUbhIcAQN>B=R00(i|+6vVs>U))C5y#QpLa$lTvdbS!lV*JcO<~P2 zjQ6+}n5$nns^E3&mP$MrnvSx9Nv`awvb)dXfoE}9K?Tf_ZWO4%79|t>j`1wrot*hm zjO=ikDmkO~cf_b#K@}$nRIvSQy|l|X2T)d*jLAEX;)b1Z7wkM zfu$`QC$LTg8-eZgH)P{#EHR$%kyA(5N!UfGmrBepLQ`lFc+bB$efhq~C3n2vYB=fp z`A0-f6LDK1M7J7rlwwipny%is-Mw(68C(`BZ~ zZ1c*O$f*v&@>i$3GL>Mm%IZ}tX!2RX*p7KpeyAF}Yd_yxaCF~Rx92x+sh+yHnNr&k z4YnVQ#vLE>;#U*EU&_jpAe82cyy<8_G_LNcYDJD>Rs1j7ew=J2y z?xhfqKfC-T5p!b}E0*3$`+>u>tYH0Dmv=XYbnLd8>Tm83`1Q3C76D`HUa{rNnj%s_ zlS?fY!c4+51Qs&pM*8v&r!dm&!Gybru$+MVBLnyi%=t1h^fCCey}%1|Kk$5KTJYPk zfBHk<%e8N=?``)P&az87Id?4B7Rw%Cd!7$$uf{SG_ssK>v{NFU05?KGhMVL4_|QxD zz$Gtf#v^GdwspLaK;x2#oqmas$`W#cXQJT{FD4WWR2<^fcFPaDvedZe3wJ5MlC)>l zL$fQ74e+Pz%I5ns3%i2?B=x#3XxgBYp{~@xv&-nbh^3u8OG4)Z-K5X9nbn*lI-M+iloWmx3vEN8tAe+bZr2lxQHZ zMu|1vCyn7rVzLH;FfTJgIIc%qW>5&5nB!83CejMdOx(=pmT@m~tGIN!QCZvs49N_3 z`S~eQ6^dYL<~ZftjKT%am7?{ALF|bEU$kaf-Pwm66H=a&&Npla?D(=1PT|)LJCu{l z7S1;3qV-_Ja5@afhbEh8kqOT?oCRJgkOYP2Mpo5V-Y4rRwY5gbgzgod>E^xZmijAq z&y1fm=ik4|Zyl=QqD%vMPkcXUxu})Yhc$BcDK${ow;8K2pQfE{k#>AoAd?K24#>1i z2f7eN~oKGV!SnC#8VT8E5DmCi1|lZ8jBA3g8|`_)u_4M zEct!#?X*_+i-Xrk+V?&EW> z9nm)hGZP8y4~Y4Jtr=Y0`||oq=S(T#Z+k!y{d{rpEWG%S>Fn+24)FQ6emVPgb3pyTTF?E0((m`Tm%dWGvXirI?e9x1Nr=*utHf_`bv}7 zqJmvRViGB)State(); + static unsigned long prev_time; static bool change_pending = false; if (reading) { change_pending = true; } if (!reading && change_pending) { - *interrupt_flag = false; - change_pending = false; - return true; + if (Milliseconds() - prev_time > DEBOUNCE_TIME) { + *p_interrupt_flag = false; + change_pending = false; + return true; + } } - } + } return false; } @@ -101,71 +108,99 @@ int main(void) { set_up_serial(); #endif - // Set static addresses for the ClearCore Controller - IpAddress local_ip = IpAddress(169, 254, 97, 177); - IpAddress remote_ip = IpAddress(169, 254, 57, 209); + // Set static address for the ClearCore Controller + //IpAddress local_ip = IpAddress(169, 254, 97, 177); + IpAddress local_ip(169, 254, 97, 177); + // Set remote (host) computer address + //IpAddress remote_ip = IpAddress(169, 254, 57, 209); + IpAddress remote_ip(169, 254, 57, 209); EthUDP eth(local_ip, remote_ip); ClearPathMC motor0(0); + + slidersystem::DataInterface command_interface; + slidersystem::DataInterface state_interface; eth.begin(); motor0.begin(); eth.send_packet(&system_status, motor0.current_velocity); + double curr_vel; + // Main loop while (true) { // Read data from the ROS2 hardware interface. - eth.read_packet(); + eth.read_packet(&command_interface); // If new data, parse for new motor control if (eth.new_data) { - switch (eth.command_data.status) { + switch (command_interface.system_status) { + case slidersystem::E_STOP: + e_stop_flag = true; + break; case slidersystem::SYSTEM_OK: // Set the new target velocity - system_status = slidersystem::SYSTEM_OK; - motor0.set_velocity(eth.command_data.vel_command); + curr_vel = -1 * motor0.current_velocity; // negative sign is flipped + if (command_interface.vel > curr_vel) { // TODO: I don't think the eth class should store the data? + motor0.set_velocity(curr_vel + 1, &system_status); + } + else if (command_interface.vel < curr_vel) { + motor0.set_velocity(curr_vel - 1, &system_status); + } break; case slidersystem::SYSTEM_STANDBY: - motor0.set_velocity(0); system_status = slidersystem::SYSTEM_STANDBY; + motor0.set_standby(); break; case slidersystem::SYSTEM_CALIBRATING: - system_status = slidersystem::SYSTEM_CALIBRATING; - eth.send_packet(&system_status, motor0.current_velocity); - motor0.calibrate(); - eth.send_packet(&system_status, motor0.current_velocity); - continue; - case slidersystem::E_STOP: - e_stop_flag = true; + // Poll the pin to see if the slider is already at the switch. // TODO: get emergency-emergency limit switches? + if (read_switch(&motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { + system_status = slidersystem::NEG_LIM; + break; + } + else { + system_status = slidersystem::SYSTEM_CALIBRATING; + eth.send_packet(&system_status, motor0.current_velocity); + motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side + break; + } + case slidersystem::NEG_LIM: + break; + case slidersystem::POS_LIM: break; - } eth.new_data = false; } // Limit switch check - if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { - motor0.set_velocity(0); - motor0.move_at_target_velocity(true); + //if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { + if (neg_lim_switch_flag) { + motor0.set_velocity(0, &system_status); + motor0.move_at_target_velocity(); system_status = slidersystem::NEG_LIM; + neg_lim_switch_flag = false; } - if (read_switch(motor0.limit_switch_pin_pos, &pos_lim_switch_flag)) { - motor0.set_velocity(0); - motor0.move_at_target_velocity(true); + //if (read_switch(motor0.limit_switch_pin_pos, &pos_lim_switch_flag)) { + if (pos_lim_switch_flag) { + motor0.set_velocity(0, &system_status); + motor0.move_at_target_velocity(); system_status = slidersystem::POS_LIM; + pos_lim_switch_flag = false; } // E stop check - while (e_stop_flag) { - motor0.set_velocity(0); - motor0.move_at_target_velocity(true); - system_status = slidersystem::E_STOP; + if (e_stop_flag) { + while (1) { + motor0.set_velocity(0, &system_status); + motor0.move_at_target_velocity(); + system_status = slidersystem::E_STOP; #if __SERIAL_DEBUG__ - ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); + ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif - eth.send_packet(&system_status, motor0.current_velocity); - Delay_ms(5000); + eth.send_packet(&system_status, motor0.current_velocity); + Delay_ms(5000); + } } // Move to target velocity (blocking) diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 82802c3..c57141c 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -6,7 +6,7 @@ */ #ifndef __SERIAL_DEBUG__ -#define __SERIAL_DEBUG__ 1 +#define __SERIAL_DEBUG__ 0 #endif #include "ClearPathMC.h" @@ -51,9 +51,9 @@ void ClearPathMC::begin() { #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Motor enabled."); #endif - // Enable pin interrupts - limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::FALLING, true); - limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::FALLING, true); + // Enable pin interrupts TODO: This is totally in the wrong place, there should be a system manager file, would also clean up main.cpp + limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::LOW, true); // What if we did LOW... would it then trigger all of the time, letting us use 'read_interrupt'? + limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::LOW, true); emergency_stop_pin.InterruptHandlerSet(&emergency_stop_callback, InputManager::RISING, true); // Wait for HLFB @@ -130,7 +130,7 @@ float ClearPathMC::get_velocity() { if (hlfb_state == MotorDriver::HLFB_HAS_MEASUREMENT) { // Get the measured speed as a percent of Max Speed float hlfb_vel_percent = motor.HlfbPercent(); - float hlfb_vel = hlfb_vel_percent * m_max_velocity_CW; + float hlfb_vel = hlfb_vel_percent * m_max_velocity; return hlfb_vel; } else { @@ -138,27 +138,66 @@ float ClearPathMC::get_velocity() { } } -void ClearPathMC::set_velocity(int vel) { - /* Set the target velocity of the ClearPath MC motor, according to maximum velocity limits */ - if (vel > m_max_velocity_CW) { - target_velocity = m_max_velocity_CW; +void ClearPathMC::set_velocity(int vel, slidersystem::SystemStatus* system_status) { + /* Set the target velocity of the ClearPath MC motor, according to maximum velocity limits. Commands are sent as positive RPM==positive direction (away from motor). In reality, positive RPM values drive the base to the direction of the motor. */ + + // Check if standby or e-stop + if (*system_status==slidersystem::E_STOP || *system_status==slidersystem::SYSTEM_STANDBY) { + target_velocity = 0; + return; + } + + // check the limit switch statuses + if (vel >= 0 && *system_status==slidersystem::POS_LIM){ + #if __SERIAL_DEBUG__ + //switch_name = "positive"; + ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); + #endif + target_velocity = 0; + return; + } + else if (vel <= 0 && *system_status==slidersystem::NEG_LIM){ + #if __SERIAL_DEBUG__ + //switch_name = "negative"; + ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); + #endif + target_velocity = 0; + return; + } + + // Correct command to speed limit + if (vel > m_max_velocity) { + target_velocity = -1 * m_max_velocity; } - else if (vel < m_max_velocity_CCW) { - target_velocity = m_max_velocity_CCW; + else if (vel < -1 * m_max_velocity) { + target_velocity = m_max_velocity; } else { - target_velocity = vel; + target_velocity = -1 * vel; } +#if __SERIAL_DEBUG__ + ConnectorUsb.Send("commanded vel: "); + ConnectorUsb.SendLine(vel); + ConnectorUsb.Send("target vel: "); + ConnectorUsb.SendLine(target_velocity); + ConnectorUsb.Send("Curr vel: "); + ConnectorUsb.SendLine(current_velocity); + //Delay_ms(1000); +#endif +} + +void ClearPathMC::set_standby() { + set_velocity(0, &system_status); } -void ClearPathMC::move_at_target_velocity(bool hard_stop) { +void ClearPathMC::move_at_target_velocity() { /* Move the motor at the set target velocity */ // Check motor status check_for_faults(); - // TODO: Handle this limit elsewhere + // TODO: Handle this limit elsewhere? // If at negative limit switch, don't let target velocity be negative if (neg_lim_switch_flag && target_velocity < 0) { target_velocity = 0; @@ -184,8 +223,8 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { for (int32_t i = 0; i < velocity_difference; ++i) { // If a flag is raised via interrupts - if ((e_stop_flag || neg_lim_switch_flag || pos_lim_switch_flag) && (!hard_stop)) { - return; + if (e_stop_flag || neg_lim_switch_flag || pos_lim_switch_flag) { + target_velocity = 0; } if (target_velocity > current_velocity) { // Toggle Input A to begin the quadrature signal @@ -219,7 +258,8 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Ramping speed, waiting for HLFB."); #endif - assert_HLFB(); + + //assert_HLFB(); // Comment out to improve function speed // Check to see if motor faulted during move if (check_for_faults()) { @@ -235,22 +275,24 @@ void ClearPathMC::move_at_target_velocity(bool hard_stop) { void ClearPathMC::calibrate() { /* Send the moving base to the motor-side (negative) limit switch */ - while (system_status == slidersystem::SYSTEM_CALIBRATING) { + while (system_status == slidersystem::SYSTEM_CALIBRATING) { + // TODO: receive message telling the calibration to be performed on the neg or pos limit switch if (neg_lim_switch_flag) { - // If limit is reached, stop moving - set_velocity(0); - move_at_target_velocity(true); + set_velocity(0, &system_status); + move_at_target_velocity(); neg_lim_switch_flag = false; system_status = slidersystem::NEG_LIM; - //_eth.send_packet(&system_status, target_velocity); - - //ConnectorUsb.SendLine(system_status); return; } - + + // Exit calibration if E-stop + if (e_stop_flag) { + system_status = slidersystem::E_STOP; + return; + } // During calibration, move toward negative limit switch - set_velocity(m_calibration_velocity); + set_velocity(m_calibration_velocity, &system_status); move_at_target_velocity(); //_eth.send_packet(&system_status, target_velocity); } diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 8cfd2bb..1d2f101 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -6,7 +6,7 @@ */ #ifndef __SERIAL_DEBUG__ -#define __SERIAL_DEBUG__ 1 +#define __SERIAL_DEBUG__ 0 #endif #include "EthUDP.h" @@ -101,7 +101,7 @@ void EthUDP::begin(void) { } -void EthUDP::read_packet(void) { +void EthUDP::read_packet(slidersystem::DataInterface* command_interface) { /* Look for a received packet, store in 'received_packet' if present */ uint16_t packet_size = udp.PacketParse(); if (packet_size > 0) { @@ -114,12 +114,12 @@ void EthUDP::read_packet(void) { m_token = strtok(received_packet_cstr, m_delimiter); if (m_token != NULL) { - command_data.status = static_cast(atoi(m_token)); + command_interface->system_status = static_cast(atoi(m_token)); // Extract second field m_token = strtok(NULL, m_delimiter); //token_cstr = reinterpret_cast(token); if (m_token != NULL) { - command_data.vel_command = atof(m_token); + command_interface->vel = atof(m_token); } } } @@ -130,17 +130,16 @@ void EthUDP::construct_data_msg(slidersystem::SystemStatus* system_status, float https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ // Reset buffers - ConnectorUsb.SendLine(status_header); memset(&msg_buf[0], 0, sizeof(msg_buf)); memset(&status_buf[0], 0, sizeof(status_buf)); memset(&data_buf[0], 0, sizeof(data_buf)); // Set data sprintf(status_buf, "%d", *system_status); - sprintf(data_buf, "%f", data); + sprintf(data_buf, "%f", data * -1); // x direction flipped in ros2 --> TODO: move all of the negative signs into one place! This shouldn't be here. // Create c-str msg - strcat(msg_buf, "{\"status\":"); // TODO: For some reason status_header gets set to 0. Needs a debugger. + strcat(msg_buf, "\{\"status\":"); // TODO: For some reason status_header gets set to 0. Needs a debugger. strcat(msg_buf, status_buf); strcat(msg_buf, ","); strcat(msg_buf, data_header); From 8706910c15ea879b21600d8f7c2409269e48c2ad Mon Sep 17 00:00:00 2001 From: lukestroh Date: Tue, 16 Jul 2024 22:53:26 -0700 Subject: [PATCH 08/21] potentially upgraded system_status var to be within ClearCoreMC class. Compiles. Hardware TBD --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 81408 -> 81408 bytes include/EthUDP.h | 5 ++- include/clearpathmc.h | 2 +- main.cpp | 46 ++++++++++----------- src/ClearPathMC.cpp | 21 +++++----- src/EthUDP.cpp | 6 +-- 6 files changed, 38 insertions(+), 42 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 95f9ff4238f8c7f6bf20c765179e7c53bcbc2438..534ee7fe2fd35650b884e4b03044ed05a3a6776d 100644 GIT binary patch delta 745 zcmZvYPiPZC7{zDS+3iM?+M2Y)q?)2hUE6dU53BIb|HI7y8EVQVUujxhgt%?`{%@LKR>feZ}R}mRd zPv434zY+N~3nrRFsndTm4dSGiiO-oaUFC2do(b|bX3|z(HNVq@T=gCH^F5g!HUgCw z!WPeG#`$~W0AJ6H`{+qbPi-oLp0e#71{HwfPZ#_6+Q+!ehCfl+-Dztw+f|}&Q=h!=Iwy2GtOipZNtt(Ez};G=JpDN* zb8$gMQWIx2k4*Z>}s5WR;bPG!C6BC delta 770 zcmZvaUr1AN6vy|wyLWCiYi`z-(_L{+=A3g|!s~LlA=SVK0)nzQ46X$RB(-=XdV;oqN8AQ`nXZ z+j247CK>9-W-v6vHG-XLwJM^<=cB4*DsK2T?o86Sl%fS=gjb{(JJW)%jo=_S35Gxv z>;rZX0w`!OfmRR(ogfBKym|t*4Y)y|9`nLB(QUJ1pdDUcJ?ew)06jnjF5m};K^%00 zY13>rTM;+_N=h@^(at+5sLZlnT37CxiqHC>h>x3I{;x#T*h`6wQoEIi zJTa#ui0@V+Ch<>+Ph<%t9&i+h`(GvfxYv}>oLOZtE#(w_TSVU?t$~>Go%Ql{%Yc=4 zv;-xdaIG+E_dJ$GMJl-bRPiKfL5fgLO)5;xobrc)`>(@U0ys3a+(Vn6V?5oev-03O zW)|}k{fYNV1kNPiQX6E7eo6Cm07>Uwu*Ns0nTg?R_q=P){`(U&ABgLINsmf;P%35K zDmNCp-y5`)gMZEV8Lg~N^Xc$eMnfMx+>>2nddyY6lxDUMue2)>%cora<^^}zx;jOf z%OyQw(oWgAJ@k{&@_dLM&B+hG9b;}jdYUVxk*7@$XP$(*UjG`M{ZYAZtj7Jmc!j?M!B diff --git a/include/EthUDP.h b/include/EthUDP.h index 26cb65d..62ba0a8 100644 --- a/include/EthUDP.h +++ b/include/EthUDP.h @@ -6,6 +6,7 @@ */ #include "ClearCore.h" #include "EthernetUdp.h" +#include "system.h" #ifndef ETHUDP_H_ #define ETHUDP_H_ @@ -52,8 +53,8 @@ class EthUDP { // Public methods void begin(); void read_packet(slidersystem::DataInterface* command_interface); - void construct_data_msg(slidersystem::SystemStatus* system_status, float data); - void send_packet(slidersystem::SystemStatus* system_status, float data); + void construct_data_msg(const slidersystem::SystemStatus system_status, const float data); + void send_packet(const slidersystem::SystemStatus system_status, const float data); }; diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 4e1a9c0..96181aa 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -60,7 +60,7 @@ class ClearPathMC { void begin(); void get_position(); float get_velocity(); - void set_velocity(int vel, slidersystem::SystemStatus* system_status); + void set_velocity(int vel); void set_standby(); void move_at_target_velocity(); diff --git a/main.cpp b/main.cpp index 8be922f..be27e35 100644 --- a/main.cpp +++ b/main.cpp @@ -65,7 +65,6 @@ volatile bool neg_lim_switch_flag = false; volatile bool pos_lim_switch_flag = false; volatile bool e_stop_flag = false; -slidersystem::SystemStatus system_status = slidersystem::SYSTEM_STANDBY; constexpr uint8_t DEBOUNCE_TIME = 10; #if __SERIAL_DEBUG__ @@ -118,50 +117,47 @@ int main(void) { EthUDP eth(local_ip, remote_ip); ClearPathMC motor0(0); - - slidersystem::DataInterface command_interface; - slidersystem::DataInterface state_interface; eth.begin(); motor0.begin(); - eth.send_packet(&system_status, motor0.current_velocity); + eth.send_packet(motor0.state_.system_status, motor0.current_velocity); double curr_vel; // Main loop while (true) { - // Read data from the ROS2 hardware interface. - eth.read_packet(&command_interface); + // Read data from the ROS2 hardware interface, store in motor command interface. + eth.read_packet(&motor0.command_); // If new data, parse for new motor control if (eth.new_data) { - switch (command_interface.system_status) { + switch (motor0.command_.system_status) { case slidersystem::E_STOP: e_stop_flag = true; break; case slidersystem::SYSTEM_OK: // Set the new target velocity - curr_vel = -1 * motor0.current_velocity; // negative sign is flipped - if (command_interface.vel > curr_vel) { // TODO: I don't think the eth class should store the data? - motor0.set_velocity(curr_vel + 1, &system_status); + curr_vel = -1 * motor0.state_.vel; // negative sign is flipped + if (motor0.command_.vel > curr_vel) { // TODO: I don't think the eth class should store the data? + motor0.set_velocity(curr_vel + 1); } - else if (command_interface.vel < curr_vel) { - motor0.set_velocity(curr_vel - 1, &system_status); + else if (motor0.command_.vel < curr_vel) { + motor0.set_velocity(curr_vel - 1); } break; case slidersystem::SYSTEM_STANDBY: - system_status = slidersystem::SYSTEM_STANDBY; + motor0.state_.system_status = slidersystem::SYSTEM_STANDBY; motor0.set_standby(); break; case slidersystem::SYSTEM_CALIBRATING: // Poll the pin to see if the slider is already at the switch. // TODO: get emergency-emergency limit switches? if (read_switch(&motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { - system_status = slidersystem::NEG_LIM; + motor0.state_.system_status = slidersystem::NEG_LIM; break; } else { - system_status = slidersystem::SYSTEM_CALIBRATING; - eth.send_packet(&system_status, motor0.current_velocity); + motor0.state_.system_status = slidersystem::SYSTEM_CALIBRATING; + eth.send_packet(motor0.state_.system_status, motor0.current_velocity); motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side break; } @@ -176,29 +172,29 @@ int main(void) { // Limit switch check //if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { if (neg_lim_switch_flag) { - motor0.set_velocity(0, &system_status); + motor0.set_velocity(0); motor0.move_at_target_velocity(); - system_status = slidersystem::NEG_LIM; + motor0.state_.system_status = slidersystem::NEG_LIM; neg_lim_switch_flag = false; } //if (read_switch(motor0.limit_switch_pin_pos, &pos_lim_switch_flag)) { if (pos_lim_switch_flag) { - motor0.set_velocity(0, &system_status); + motor0.set_velocity(0); motor0.move_at_target_velocity(); - system_status = slidersystem::POS_LIM; + motor0.state_.system_status = slidersystem::POS_LIM; pos_lim_switch_flag = false; } // E stop check if (e_stop_flag) { while (1) { - motor0.set_velocity(0, &system_status); + motor0.set_velocity(0); motor0.move_at_target_velocity(); - system_status = slidersystem::E_STOP; + motor0.state_.system_status = slidersystem::E_STOP; #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif - eth.send_packet(&system_status, motor0.current_velocity); + eth.send_packet(motor0.state_.system_status, motor0.current_velocity); Delay_ms(5000); } } @@ -207,6 +203,6 @@ int main(void) { motor0.move_at_target_velocity(); // Send status, velocity data to the ROS2 node. - eth.send_packet(&system_status, motor0.current_velocity); + eth.send_packet(motor0.state_.system_status, motor0.current_velocity); } } \ No newline at end of file diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index c57141c..087d37d 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -15,7 +15,6 @@ extern volatile bool neg_lim_switch_flag; extern volatile bool pos_lim_switch_flag; extern volatile bool e_stop_flag; -extern slidersystem::SystemStatus system_status; ClearPathMC::ClearPathMC() {} @@ -138,17 +137,17 @@ float ClearPathMC::get_velocity() { } } -void ClearPathMC::set_velocity(int vel, slidersystem::SystemStatus* system_status) { +void ClearPathMC::set_velocity(int vel) { /* Set the target velocity of the ClearPath MC motor, according to maximum velocity limits. Commands are sent as positive RPM==positive direction (away from motor). In reality, positive RPM values drive the base to the direction of the motor. */ // Check if standby or e-stop - if (*system_status==slidersystem::E_STOP || *system_status==slidersystem::SYSTEM_STANDBY) { + if (state_.system_status==slidersystem::E_STOP || state_.system_status==slidersystem::SYSTEM_STANDBY) { target_velocity = 0; return; } // check the limit switch statuses - if (vel >= 0 && *system_status==slidersystem::POS_LIM){ + if (vel >= 0 && state_.system_status==slidersystem::POS_LIM){ #if __SERIAL_DEBUG__ //switch_name = "positive"; ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); @@ -156,7 +155,7 @@ void ClearPathMC::set_velocity(int vel, slidersystem::SystemStatus* system_statu target_velocity = 0; return; } - else if (vel <= 0 && *system_status==slidersystem::NEG_LIM){ + else if (vel <= 0 && state_.system_status==slidersystem::NEG_LIM){ #if __SERIAL_DEBUG__ //switch_name = "negative"; ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); @@ -187,7 +186,7 @@ void ClearPathMC::set_velocity(int vel, slidersystem::SystemStatus* system_statu } void ClearPathMC::set_standby() { - set_velocity(0, &system_status); + set_velocity(0); } @@ -275,24 +274,24 @@ void ClearPathMC::move_at_target_velocity() { void ClearPathMC::calibrate() { /* Send the moving base to the motor-side (negative) limit switch */ - while (system_status == slidersystem::SYSTEM_CALIBRATING) { + while (state_.system_status == slidersystem::SYSTEM_CALIBRATING) { // TODO: receive message telling the calibration to be performed on the neg or pos limit switch if (neg_lim_switch_flag) { - set_velocity(0, &system_status); + set_velocity(0); move_at_target_velocity(); neg_lim_switch_flag = false; - system_status = slidersystem::NEG_LIM; + state_.system_status = slidersystem::NEG_LIM; return; } // Exit calibration if E-stop if (e_stop_flag) { - system_status = slidersystem::E_STOP; + state_.system_status = slidersystem::E_STOP; return; } // During calibration, move toward negative limit switch - set_velocity(m_calibration_velocity, &system_status); + set_velocity(m_calibration_velocity); move_at_target_velocity(); //_eth.send_packet(&system_status, target_velocity); } diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 1d2f101..27fa34c 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -125,7 +125,7 @@ void EthUDP::read_packet(slidersystem::DataInterface* command_interface) { } } -void EthUDP::construct_data_msg(slidersystem::SystemStatus* system_status, float data) { +void EthUDP::construct_data_msg(const slidersystem::SystemStatus system_status, const float data) { /* Construct the message to send to the ROS2 Node on the host computer https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ @@ -135,7 +135,7 @@ void EthUDP::construct_data_msg(slidersystem::SystemStatus* system_status, float memset(&data_buf[0], 0, sizeof(data_buf)); // Set data - sprintf(status_buf, "%d", *system_status); + sprintf(status_buf, "%d", system_status); sprintf(data_buf, "%f", data * -1); // x direction flipped in ros2 --> TODO: move all of the negative signs into one place! This shouldn't be here. // Create c-str msg @@ -148,7 +148,7 @@ void EthUDP::construct_data_msg(slidersystem::SystemStatus* system_status, float } -void EthUDP::send_packet(slidersystem::SystemStatus* system_status, float data) { +void EthUDP::send_packet(const slidersystem::SystemStatus system_status, const float data) { /* Send a packet */ construct_data_msg(system_status, data); udp.Connect(m_remote_ip, m_remote_port); From 0599c5404b8b52e1096b71f265ed74cd1192f395 Mon Sep 17 00:00:00 2001 From: lukestroh Date: Tue, 16 Jul 2024 23:00:30 -0700 Subject: [PATCH 09/21] extended switch/case --- main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/main.cpp b/main.cpp index be27e35..2c23c09 100644 --- a/main.cpp +++ b/main.cpp @@ -135,7 +135,7 @@ int main(void) { case slidersystem::E_STOP: e_stop_flag = true; break; - case slidersystem::SYSTEM_OK: + case slidersystem::SYSTEM_OK: case slidersystem::NEG_LIM: case slidersystem::POS_LIM: // Set the new target velocity curr_vel = -1 * motor0.state_.vel; // negative sign is flipped if (motor0.command_.vel > curr_vel) { // TODO: I don't think the eth class should store the data? @@ -161,14 +161,16 @@ int main(void) { motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side break; } - case slidersystem::NEG_LIM: - break; - case slidersystem::POS_LIM: - break; + //case slidersystem::NEG_LIM: + //break; + //case slidersystem::POS_LIM: + //break; } eth.new_data = false; } + /* TODO: Still need to do some polling of the limit switches somewhere in here? */ + // Limit switch check //if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { if (neg_lim_switch_flag) { From d7e779f5f4c66188dabf9972470dcfeb6b7d2d8c Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 18 Jul 2024 16:02:34 -0700 Subject: [PATCH 10/21] got the limit switches to stop without inhibiting movement in other direction. Needs to be extensively tested still --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 81408 -> 88064 bytes include/EthUDP.h | 4 ++-- include/clearpathmc.h | 1 - include/system.h | 5 ++-- main.cpp | 24 +++++++++++++++---- src/ClearPathMC.cpp | 23 +++++------------- src/EthUDP.cpp | 26 ++++++++++++++++----- 7 files changed, 49 insertions(+), 34 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 534ee7fe2fd35650b884e4b03044ed05a3a6776d..f061942c5c997a09372581588b7b80c11abce191 100644 GIT binary patch delta 2790 zcmd6pe{fXQ702Is`*yQr7hHaYkc6=LAqn}hNnitMOaft7)DX!onxN2$!C)Ymrh$f* z#3EaCsIk;$_n^;Fhu|}=DB^^@O2%Jp<%dHBOOPp*76<&%j^j|J&f+vyNxyfqOqn*- zI^!R{na`f{?mg$c`|i2tybXMz1qRH$6;o3bMKOU(BO@aa3W1`1{#4ZxXuj}4^J(6o zZl(A5qFQH##YiruIgnhyn@qd6RN=-3(!qG(0M#H7*ufMaon}GG_!A)0z*Qg{lmaWT zfEgea=|R)X(?Mj#DtgKPq; z!0li)kmGki%6FLyxfZm7b>L3W1~vx$S=1f$Po^#S(E=C>scn|xjXgtOk&J}?STf`T z#!84BmkF5&GQj18$npP7NCM_HFjhjOy-e;H3E85QVd9%6!m{5dG^0y%R@^w{>OUuVQUSG~js zO(kZzPmghzqlP_8OK1&Wbez;=wV)JpIXbcKYmG#7a)E`NExR>Y8>4P5a8paTbB#}v zmB@EbEwCMhvW)P9`^Qh@^M&JX(U^9C*qM?wYMoLyjuv2Amda?Hlz5!fIx{VjqX=Z) z3FU1uJae|nnW>ppS$WlQ57C*sog$eYGRJZ+&ABt{W^G}7-WG(iAK>DV+q~mq1uyG3 z_Poxf8cFf?Ibs6q-M&6nqR~0r+lJs2_L$lg3tANuau$fPT|bV z`hQj+M)FBD(p7%zR9J+ISCCPqMz&s0d}I}teUFb1nTl<@@UXM8Jt>M9DIrx4mQuB- zF8hr+H0y{a`{3wS&5CbT4mPm{qq4=w8S^ioJS&D5Bxs=&&o|#_9?q)MWZN9wk8GbN zd#v195zV#}8*kN!n(OzQB6;mJd46d5>cqZZRv*v4biVeVe1oDy__e$(A)nW}1YWX{ zj)w7JwN;fNzenya2mGFKv38#kH?Ya0)B|woG=-LP!`gMU4YBGu{I|56X61*<2IUU4 z#SXN{Xme`W=iJ^J;!i(I5=qNG8c+8H{WIxT zL4WG_UIdrtAp~{?{Y8a+P-F!?4rzcVKtFgAguzo_H+UL6Bi4IYl5x8hTsDa(xm<;&3&c)n36w{~a94sF z+U5|B?zz|$**)$WvdV)mKMA9!W~on;pVU}cN0<7n@&l6{pSAd*cvHE1bBz_}F7`o+ zc||uDuyfY#D@tYiarXF1IGR%+oLkS~2%pH~q2Uz1(CXml{+uWp?=#5QrW!jfk={WpkB{Ido1F;J?YxmPW>w1|D;>Ej~pX-+q`I`vUD`%O+GvDFzGr#+nn%I-*->TU0SEn{nZrSB)wp7H>Z}+Ok5mjtH=rwWg=>{J7B8dmy z^zodNjiND%t$4$3oN*YZ)X2xGRo_9B$&L9A;eH!0`46k{CsiLxBfGGlk5X8p37mOT zn`rxN2eIQs`C$1Jdflc^QYl$)Cn|fvM+2sCGHKylO4^@Iy(*vTTZbOn6lXCnd_XhH ze^(89Mh?~Lf5@dD^I-WS`jXl7iN3Fh&QP74nWj4`s7U`=9(C%+@~J`pdokUm-(Ntl zaAD;N-I+tJ`r<+g=%2XgfYe&`^||osaZwBVuUW0ns6?Rk1vFj1R7#bDPnQszq*|dT kms8r{W2H1q`o%o@<6u!4?IpdUl$r+J<+R7dH#TJa3+{SpL;wH) delta 2677 zcmb`IdrZ?;6vumi{S^uYECovi#EJ-2QGtn~QK*2Z=zwZ{EX!D&nHp4d+Dy@j#rd8( z+oP+m&G?HtGm}O6ZICV7MASvI`HDI*b5pa#Kg^=I)l8hqzNan37#~Zvn|yMgJ-^fQ zJ@;JqA)Wi7ym@$6i$*htb&;n_NxJLJ+a=jT_Hb8Dm zCN0(;Se}lF0hmBEhy>$67mx#@Kn!5pM)dtbGUx-+Kp4PQpmoPM3G@YppcjY-k3#!w zqbug@z(A0q(Nt+(!5|a#13f?<7yt%?48R`SK^(Ax1i<#PLj9mnANodn>{D}nXg=KI z=+(dMiOq71E5L-%+C=nDFbPZsZ+IL@V!FqXAZk30j4{<%ng`~CcfbO$5V*j*fWN;8 z{bIm>w0CIjJ@m^!EvN(ZP|OdrpILP}{Q0_s?+xg}CRrQqS(`-gv^KoQ?MZET&f3U; zUFnAYMcQ!qpVFp~#R+J`#yYeag0&~L$;I3bp4284bJk{%#}U^p9|JZJ6Y9f5{eKi= z{Qo5e`{s=tBp-!elo$D<%wZtC!4=Je&Ze$29F$&mh}L!anvO}?x}h+(nQDxsJQGXl zp78?JcFiPzZ261!RexM&7+F(#(J^f{xl+m_0O2@UP}`_-nXgXZ zQ?8NtNxNG;mtoObCn4?;&#fMhQXr+0UyX;mr2&tG(BJ2g3zYwO5GqR=Srno zIo>GZ^v#ApZHs3%?@2#!&=S75Orxo%kwcd&nUZ&gJ+{65=GFAuHLtFit$%IYjuxNR zH!V*LRNXV&LdlYp!IC$>h{H#&eCNOM#cY#DbvkJdg|4Jy#cvL}?MlE-F;>TfcA}aw zI0Lx(Jh@_|Ix_EwPRWrJyXfz@CQSO-|0_2@qU8^A`e2|V^pi|exm zgRS5r-~-#hcCZ8N1iQe;;PK&U9}4>XO_OSN-V&s=l&Ym;8noV7pT%z4-F74DdVK!f zyy^R>am1oN%7LeDpoO95*YHW!12cv`Z2Y(&6S zuH!)wGaz1B8Xd2Li83S}{}Fx>f_j%BRyiZcT@C@X*#?%F|1FyDu2CeY<_Wi=z3D{} zVl0-p*aVHHpAF_Km#)xf%Bd(fJBsfX)&fm=OZFz74Emc0Ex4p`nF;2iCsvZjWqk9(Ks{Kq>#z1cVEca7jfzBQ&jEnl6~B2>iK${uXVV{ zP@Cr4g=d0^Ofv?&Fwv?dDm>_D2YD=|Eyv*{PVPBOobu>S+oLyl%OG_IBrhv^ zaWd!v$)L8?!N(=Y0DfjNIt?ue)rKX=1bz=Y3ygnhvT9u2itN7Jm(H~%Qmey6#=rGy zpP2%-+eYhQwoC|E{=zeZ(K7u`ixfX>%R&Y+vtJk zH&IVw)>hwCt4N_WG>L9f;xoUeY>}0State(); +} + int main(void) { #if __SERIAL_DEBUG__ @@ -120,7 +124,7 @@ int main(void) { eth.begin(); motor0.begin(); - eth.send_packet(motor0.state_.system_status, motor0.current_velocity); + eth.send_packet(&motor0.state_); double curr_vel; @@ -157,7 +161,7 @@ int main(void) { } else { motor0.state_.system_status = slidersystem::SYSTEM_CALIBRATING; - eth.send_packet(motor0.state_.system_status, motor0.current_velocity); + eth.send_packet(&motor0.state_); motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side break; } @@ -196,15 +200,25 @@ int main(void) { #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif - eth.send_packet(motor0.state_.system_status, motor0.current_velocity); + eth.send_packet(&motor0.state_); Delay_ms(5000); } } + if (!poll_switch(&motor0.limit_switch_pin_neg)) { // This is now working as it should?? Can it be? + motor0.state_.system_status = slidersystem::NEG_LIM; + } + else if (!poll_switch(&motor0.limit_switch_pin_pos)) { + motor0.state_.system_status = slidersystem::POS_LIM; + } + else { + motor0.state_.system_status = slidersystem::SYSTEM_OK; + } + // Move to target velocity (blocking) motor0.move_at_target_velocity(); // Send status, velocity data to the ROS2 node. - eth.send_packet(motor0.state_.system_status, motor0.current_velocity); + eth.send_packet(&motor0.state_); } } \ No newline at end of file diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 087d37d..ea04f86 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -51,8 +51,8 @@ void ClearPathMC::begin() { ConnectorUsb.SendLine("Motor enabled."); #endif // Enable pin interrupts TODO: This is totally in the wrong place, there should be a system manager file, would also clean up main.cpp - limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::LOW, true); // What if we did LOW... would it then trigger all of the time, letting us use 'read_interrupt'? - limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::LOW, true); + limit_switch_pin_neg.InterruptHandlerSet(&neg_lim_switch_callback, InputManager::FALLING, true); // What if we did LOW... would it then trigger all of the time, letting us use 'read_interrupt'? + limit_switch_pin_pos.InterruptHandlerSet(&pos_lim_switch_callback, InputManager::FALLING, true); emergency_stop_pin.InterruptHandlerSet(&emergency_stop_callback, InputManager::RISING, true); // Wait for HLFB @@ -180,7 +180,7 @@ void ClearPathMC::set_velocity(int vel) { ConnectorUsb.Send("target vel: "); ConnectorUsb.SendLine(target_velocity); ConnectorUsb.Send("Curr vel: "); - ConnectorUsb.SendLine(current_velocity); + ConnectorUsb.SendLine(state_.vel); //Delay_ms(1000); #endif } @@ -196,22 +196,11 @@ void ClearPathMC::move_at_target_velocity() { // Check motor status check_for_faults(); - // TODO: Handle this limit elsewhere? - // If at negative limit switch, don't let target velocity be negative - if (neg_lim_switch_flag && target_velocity < 0) { - target_velocity = 0; - } - - // If at positive limit switch, don't let target velocity be positive - if (pos_lim_switch_flag && target_velocity > 0) { - target_velocity = 0; - } - // Determine which order the quadrature must be sent by determining if the // new velocity is greater or less than the previously commanded velocity // If greater, Input A begins the quadrature. If less, Input B begins the // quadrature. - int32_t curr_velocity_rounded = round(current_velocity / velocity_resolution); + int32_t curr_velocity_rounded = round(state_.vel / velocity_resolution); int32_t target_velocity_rounded = round(target_velocity / velocity_resolution); int32_t velocity_difference = labs(target_velocity_rounded - curr_velocity_rounded); @@ -225,7 +214,7 @@ void ClearPathMC::move_at_target_velocity() { if (e_stop_flag || neg_lim_switch_flag || pos_lim_switch_flag) { target_velocity = 0; } - if (target_velocity > current_velocity) { + if (target_velocity > state_.vel) { // Toggle Input A to begin the quadrature signal motor.MotorInAState(true); // Command a 5 microsecond delay to ensure proper signal timing @@ -250,7 +239,7 @@ void ClearPathMC::move_at_target_velocity() { } // Update the current velocity - current_velocity = target_velocity; + state_.vel = target_velocity; // Wait for High-Level Feedback (HLFB) to assert (signaling if the motor has reached // its target velocity) diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 27fa34c..6616ea4 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -105,7 +105,7 @@ void EthUDP::read_packet(slidersystem::DataInterface* command_interface) { /* Look for a received packet, store in 'received_packet' if present */ uint16_t packet_size = udp.PacketParse(); if (packet_size > 0) { - uint32_t bytes_read = udp.PacketRead(received_packet, MAX_PACKET_LENGTH); + udp.PacketRead(received_packet, MAX_PACKET_LENGTH); new_data = true; // Parse data from the received packet @@ -114,18 +114,25 @@ void EthUDP::read_packet(slidersystem::DataInterface* command_interface) { m_token = strtok(received_packet_cstr, m_delimiter); if (m_token != NULL) { + //#ifdef __SERIAL_DEBUG__ + //ConnectorUsb.SendLine(m_token); + //#endif command_interface->system_status = static_cast(atoi(m_token)); // Extract second field m_token = strtok(NULL, m_delimiter); //token_cstr = reinterpret_cast(token); if (m_token != NULL) { + //#ifdef __SERIAL_DEBUG__ + //ConnectorUsb.SendLine("got far"); + //ConnectorUsb.SendLine(m_token); + //#endif command_interface->vel = atof(m_token); } } } } -void EthUDP::construct_data_msg(const slidersystem::SystemStatus system_status, const float data) { +void EthUDP::construct_data_msg(slidersystem::DataInterface* state) { /* Construct the message to send to the ROS2 Node on the host computer https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ @@ -135,8 +142,8 @@ void EthUDP::construct_data_msg(const slidersystem::SystemStatus system_status, memset(&data_buf[0], 0, sizeof(data_buf)); // Set data - sprintf(status_buf, "%d", system_status); - sprintf(data_buf, "%f", data * -1); // x direction flipped in ros2 --> TODO: move all of the negative signs into one place! This shouldn't be here. + sprintf(status_buf, "%d", state->system_status); + sprintf(data_buf, "%f", state->vel * -1); // x direction flipped in ros2 --> TODO: move all of the negative signs into one place! This shouldn't be here. // Create c-str msg strcat(msg_buf, "\{\"status\":"); // TODO: For some reason status_header gets set to 0. Needs a debugger. @@ -145,12 +152,19 @@ void EthUDP::construct_data_msg(const slidersystem::SystemStatus system_status, strcat(msg_buf, data_header); strcat(msg_buf, data_buf); strcat(msg_buf, footer); + + #ifdef __SERIAL_DEBUG__ + ConnectorUsb.SendLine(msg_buf); + //ConnectorUsb.SendLine(motor0.command_.system_status); + //ConnectorUsb.SendLine(motor0.command_.vel); + //ConnectorUsb.SendLine(motor0.state_.vel); + #endif } -void EthUDP::send_packet(const slidersystem::SystemStatus system_status, const float data) { +void EthUDP::send_packet(slidersystem::DataInterface* state) { /* Send a packet */ - construct_data_msg(system_status, data); + construct_data_msg(state); udp.Connect(m_remote_ip, m_remote_port); udp.PacketWrite(msg_buf); udp.PacketSend(); From 8973e82f5977a2a4a31e3b82b85e12c8cdfe9f2e Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 18 Jul 2024 19:30:48 -0700 Subject: [PATCH 11/21] made some additional edits to the flow of the program --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 88064 -> 97280 bytes include/interrupts.h | 3 +- include/system.h | 2 +- main.cpp | 32 +++++++++++++++------ src/ClearPathMC.cpp | 12 ++++---- 5 files changed, 32 insertions(+), 17 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index f061942c5c997a09372581588b7b80c11abce191..f6adcb0d6d2cf0b905ae75b4d50be200292e4816 100644 GIT binary patch delta 3679 zcmbW3dsI}{6^HM>%)ksY$neG&0umUgjDQNLhy#O09uY~28#UPgLP2B_sf3QmC47m6L;?{-L=uAu-e(-y@5v4&%lq*p z`&6GFOI9lgp2Bc)LUF4sBykjZNrX2MNca)`3Eou|*@1mN&j*noY&C}XjD8^4&uSbR zKmLILuUSC(DSff2WS=Dpi6Y`Tt8pOAqHdtVYK)mQfI_?pPa)*T6V-&7SVZvnVzMt1 zONf{HVlR_jN-QH@A@D9#$sK?Fcdy!Thjn?zK%JgScn^Kjt)n<>!w$uJp zySvoa_XN;3!&|<&_?ejK$)&mGg|(U-g@#;_Kuf6B6Upg}z}uQyxQ#V_Q*l|)Fz;>( z@{L7x?)gsmzuQAM>k5b$F*3}q&{HRT*#CZnNZ zJc@OhifpRGx4!coP2{x1V{Kiw(z%KGvg(~r%rF){QVTMEx;P`|6&6(^Wp?u=CJ3x2 z2t-@7K?!`B^(S~Ocpht_>tH%|MNh>)$E2bq#?n->4}6La6{*j*sW@F3574zzSj|@hb5e@j9`Z;N@$`zCqLzZxZ*cGeTnDB4-1! zk&uW@#M?vzv6D62jkDEY z*NaN`+O^IY$fDF8(3G483cl@(yk=I)NQJ8kT2d0!6%?%`7V**pDT8E}xYLmmg-KtP zVrkm|zZ18YfAZq)lfP})SpEK&f6!bOti}M`(smgwoAr3OU61zjIV@+=ey~pQXZ~|G ziGI_@RtcLzKP^1)ZAy2pcBemY!P?4`z!o`4X5jtPH)Q6>dXU6GC}KqgyHxz|B)t*} zp0{%Fag`Kf%RkWRS_9-&#NtmJSlDPiJQ)3G!!)bH)-V-L;j znBN}1zC}Gyb7GzIjT`dL21NM=7EJr2X~%8Nf;)SCKR7A~O*ExC`)tuxd02j>CGD>B zRnJ#y;R)VHqgExW`faW#Tsbd7ebDPz3Oh9{0qnmMZDaJBo}PNozYXs+6b7a{UM)X) z5REM1hYi+`sRDbY%r@De6!Pd5xDdVKPRtJt1G_-&cm_i3KwLzu}6o z8xz&Pc!}7evlKtKEp15#ete-hDWC5{)QoU6)f#{qmL3#sBQ&U~9ifE5G@BUr^lI~+ zEpEKrt_1$i;=g?UQg~yj<%v$297P~CaUC)$zTPR%lk=QA)ja?mMt{`YQLxgLcCf{T zV|e=evH)JeIgaJ5EVM5p=i7NV>weu!V}DJwH^^*RR9icgi_6T~dO-WRIDE8A9KS}+ zx>etAUL+P%=g{R+wLhv^T^Mx{DhF}s5!UB4h(GBe}!9|iI3>JaY zRN~%DMUWy-SS(}<1%ga)yXBI`51z$fV<1$y+(A(5a-(tW+99ldZIA~`{ChF5;>Mlc z@4oW^*f)UvkT>i`*ia{#{6Ne8vdatnh0#I^U0!rT7R_p=P(!*Lk;dCk_@n-BX!Fqyp$WJYKiZ? zJ$ea$!tJe_&~YIFzUcLiDb#Z-&1EK2O_jM?SB{N4wCszHU4YwK+o9c6M=`-%UYJ$T zTYf(*%^wE}-o%$grk`x$bDg#w%guT|iq8HY5l{bqI1XX< zj$;aE89bb2|2^&l%yg{;c7TkvIdUPG-}hW8vd_u-abfyBBvr9%FrNnnbVt4KN7?n` zzI^2O#9*asII$*vOgbFz&-c{Y{efUsLYuTO7>t;3cRuY74NW_~dEsCf z0vIj!LpmMgBdMoKDMANYX+b>brGI8aNb@Q!RERLg9ZvSFHk;;Fz92OwLO^p@92iB6 zA6G1e>tL?5G@BlK;;Gp+9c%;1HWr?iPU&EXG%E*YH(yVH<8m<2B8dyhTbx}`;(Sz_ zW|hoZU@9xAnprakjky8Rfr*eR?R^?#(#Hj0Z{CmsIiAvmX%KAbR}|cJ$1B delta 2452 zcmdUvdr*{B6u|d<`zCzH{!q=id9> zbIv^#znd!RP3wkr?rJa?jKqW1)>aY*AmVpBx+$iLqFYysaDlQ?X*9^YN+f6<)N&I| zlVxiP$p9jf2qG**8sSd_5}gRPiy_HdcOcoF=t$UzfrOdxA$kzqh*%<&h$eayygrO1 zukTDUj?E)TvEn8*yiY&U1`+pZI+0`-q7Pvw1{1@H1R~t!2#<&&C4}HTxuv)17?|0; zM|NB1Ig&FUGh)LDwEBg78k|T>A|?}4h^Yh{Od~m+C?E=n83d2NM3Vn}0Lhn$*+elh zhbSQyxg60@=5mBWIepX$8q$;!Q^KOEvtF-5qrBtu43x8{dB1=1HjlHngNa^5ySI7# zNpJhoyh{9&x7nT}_k_3E*@_{Vy`BFwZx5lR?cV0CIC$X%>+S7-5dgP;C0ICUSLyFr za>MF?WQK9`1)|WP!6Y+%!tzot15wthw zJs*e7VQ1Bm+*#hauc?NVi?XGsucGCQQTZLSjdbR$2KV%IFnOcPmgp?V^#e}H)HWkO z7b$xQ1KwXU+RV?BA0n>@2QIRgLmu9-k5jowcn@~=S*_WuXpM}<2Bp7r)~p82YL(XM z(N8@m=+QZ}Q7yvNCLHwhNQ{~NwcbzF`Zi!g>2P2bIgw!_#`_;c;U^;1(MA zhKmR_cg8RaMM#2JyFqW=(hjver#Eot8)2!_}|Vvm1`wCT{b%dDV!IxWY(Cy zpkD<{U66{G6a2-z*I=ore+7DA(|0|z!o|V2-}`vZw9QS9dfTDj!anH|W1!N48w$^d z4%Q~j@ZbDy+UK?h*R%h?vE&6z8(6NDru)IObWYExb@$CU(h zR_B?mw0#u8my^xafuPS*DNbTVA>_!M`EK}2YfCIG?zlIzqaUq$Js0?iqI215eW~hA z=v8%(r^b1;-K8c@iDR-QIu}&Vz=!eeT@irPY0F9R?O-Uv4v99bnHh!CeIw+NH&a2c zR`g1XXdDcMf1lf+>Xkn3T0celt+{2y`~!7(WtSa4(>mdSlfU9I43Ui(sp8Rnbg6oG zfWASI3+`2ctUA2JtZ!8GQz8C)DVI6rj0QaSQ@pmXSWFuYW=y$hmv!Io1kB#u;mHZ3 z(U9@@m`u5OQw5X=<{V0Q-NK=sA)EXF#qw zQ3{?HgJ8RY+fQ$jYffc>ent^mFRTgot_ovnMqfj+n?TL-F!Ji^$4{42&9A z;wcG%^?;TOLp>++VW=Oyp#@Rk5K~hjLF5;~22mOVcVUe<7XwQmUEH1w6FvD>sPi=e zIV({tFNP9vZ7hU)Drdl4#WQynI25sLEX)=^7DKo=G#eVkv{zxYxK;u^#r`pHOpMQf iOCHl)i1iT%MnSTu9}TMK$U?a8Bi@(;y|$%5*FOP;Fdk6= diff --git a/include/interrupts.h b/include/interrupts.h index 23dd5db..ee6830e 100644 --- a/include/interrupts.h +++ b/include/interrupts.h @@ -9,7 +9,8 @@ #ifndef INTERRUPTS_H_ #define INTERRUPTS_H_ -//#include "system.h" +#include "system.h" + extern volatile bool neg_lim_switch_flag; extern volatile bool pos_lim_switch_flag; diff --git a/include/system.h b/include/system.h index 8f7533b..726d0eb 100644 --- a/include/system.h +++ b/include/system.h @@ -29,7 +29,7 @@ namespace slidersystem }; struct DataInterface { - slidersystem::SystemStatus system_status = SystemStatus::SYSTEM_OK; + SystemStatus system_status = SystemStatus::SYSTEM_OK; double vel = 0.0; }; diff --git a/main.cpp b/main.cpp index 9efa7af..20f439d 100644 --- a/main.cpp +++ b/main.cpp @@ -139,7 +139,7 @@ int main(void) { case slidersystem::E_STOP: e_stop_flag = true; break; - case slidersystem::SYSTEM_OK: case slidersystem::NEG_LIM: case slidersystem::POS_LIM: + case slidersystem::SYSTEM_OK: // set_velocity() deals with case, but with state_. Is it worth doing an additional check here? Looks like it's faster... // Set the new target velocity curr_vel = -1 * motor0.state_.vel; // negative sign is flipped if (motor0.command_.vel > curr_vel) { // TODO: I don't think the eth class should store the data? @@ -165,16 +165,31 @@ int main(void) { motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side break; } - //case slidersystem::NEG_LIM: - //break; - //case slidersystem::POS_LIM: - //break; + // check the limit switch statuses + case slidersystem::NEG_LIM: + curr_vel = -1 * motor0.state_.vel; // negative sign is flipped + if (curr_vel <= 0){ + #if __SERIAL_DEBUG__ + ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); + #endif + motor0.set_velocity(0); + return; + } + break; + case slidersystem::POS_LIM: + curr_vel = -1 * motor0.state_.vel; // negative sign is flipped + if (curr_vel >= 0){ + #if __SERIAL_DEBUG__ + ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); + #endif + motor0.set_velocity(0); + return; + } + break; } eth.new_data = false; } - /* TODO: Still need to do some polling of the limit switches somewhere in here? */ - // Limit switch check //if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { if (neg_lim_switch_flag) { @@ -201,7 +216,8 @@ int main(void) { ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif eth.send_packet(&motor0.state_); - Delay_ms(5000); + return; + //Delay_ms(5000); } } diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index ea04f86..0629753 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -147,22 +147,20 @@ void ClearPathMC::set_velocity(int vel) { } // check the limit switch statuses - if (vel >= 0 && state_.system_status==slidersystem::POS_LIM){ + if (vel <= 0 && state_.system_status==slidersystem::NEG_LIM){ #if __SERIAL_DEBUG__ - //switch_name = "positive"; - ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); + ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); #endif target_velocity = 0; return; } - else if (vel <= 0 && state_.system_status==slidersystem::NEG_LIM){ + if (vel >= 0 && state_.system_status==slidersystem::POS_LIM){ #if __SERIAL_DEBUG__ - //switch_name = "negative"; - ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); + ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); #endif target_velocity = 0; return; - } + } // Correct command to speed limit if (vel > m_max_velocity) { From 3b6231d55c78072be3a2847b9ef2d2c402f8b6fc Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 18 Jul 2024 19:30:55 -0700 Subject: [PATCH 12/21] made some additional edits to the flow of the program --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 97280 -> 97280 bytes include/EthUDP.h | 8 ++--- include/system.h | 4 --- main.cpp | 33 ++++++++------------- src/ClearPathMC.cpp | 7 +++-- src/EthUDP.cpp | 4 +-- 6 files changed, 23 insertions(+), 33 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index f6adcb0d6d2cf0b905ae75b4d50be200292e4816..536d38a7d837c4cd33867304a58dbafb5cb435b0 100644 GIT binary patch delta 695 zcmajb%}X0W7zXg2aVN&Qi7_#$#WpTU8d9;ET0d$j6vb1k5$nl|U?@3AEJSk<4^2q_ zfU-M<9(pMwC_)cq=h%aw?Nv&_LrW1UJ@iz1=)r>@eO^hW2c^UQcwcsgclO!pfv6sc zw?!_p-#ebwbcwGsH)vK)utJ7mG{Z6a0BRa0VH7NgfFFX8g&ueaEl>g*e2{=^Kr_2g zk5H4SZEyvy1KrVwnt@)!kgv?>@BsQdFa%+ULMz+=nnmkwy-%TbKnyNBQxSP+TI`ZD zm6YA$g-Ivnd$D1<(}FTSXRbdqM|V<2+NBIs|Cd+P&z+a|c75+}UQ?F*k-$~FJxeGZ zv-31pKi&>PBmA3-{t4xvT$G8viPpKf1nu0EADZsVAI+`KLW@pWUzrMVHOf_tdtuv@ zc04J!`7K$FPYO1UiSlW*TmOz*%zfP@f9Yh*beV)~``4EdVOjq9ReA$^PENyKr2Ug35;mMJKiW>8&ey+x2?&6q#0ylNi zZPoUSE-$`e+J3yxoJV0De(;ld`6m~TDRMQ>RpInOU*<1sd6v_;Zyr-cURzmPVf+sp CmddgK delta 847 zcmZ{iUr19?9LMkPbnl$ootxX5|4MJV;wF)6O>JZ~Z6T!1R)_>4C=%gxH5o!mIM%-x zU$zrmDCpr3hR}m}o_ZLJ(kCM-C<`L&Ef7Q^LNcx2so6_*;KTQO&hMUc@Ar3ZYE4P4 zDKkSvSv|Hg9^(n}2qrX1l78ZFpjr_pVng;JUC1WZR8$vYM%s`g$U&q7u_Fn@hj@`% zWH%zttVi8ItwR;(m{9i?{C-qjlBgk9GzQQ&B7VezxRFw%0ukq2MXf4$G2e;alZlqm zAF>K1nP?q7pxl+qaFxy})AIA>@fb~z8)RML-%ORzr(i|AUyKlXq1$B@>msYrB2aRnHzcd-En1o#y curr_vel) { // TODO: I don't think the eth class should store the data? - motor0.set_velocity(curr_vel + 1); + //curr_vel = -1 * motor0.state_.vel; // negative sign is flipped + if (motor0.command_.vel > motor0.state_.vel) { + motor0.set_velocity(motor0.state_.vel + 1); } - else if (motor0.command_.vel < curr_vel) { - motor0.set_velocity(curr_vel - 1); + else if (motor0.command_.vel < motor0.state_.vel) { + motor0.set_velocity(motor0.state_.vel - 1); } break; case slidersystem::SYSTEM_STANDBY: @@ -155,35 +153,32 @@ int main(void) { break; case slidersystem::SYSTEM_CALIBRATING: // Poll the pin to see if the slider is already at the switch. // TODO: get emergency-emergency limit switches? - if (read_switch(&motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { + if (!poll_switch(&motor0.limit_switch_pin_neg)) { motor0.state_.system_status = slidersystem::NEG_LIM; - break; } else { motor0.state_.system_status = slidersystem::SYSTEM_CALIBRATING; eth.send_packet(&motor0.state_); motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side - break; } + break; // check the limit switch statuses case slidersystem::NEG_LIM: - curr_vel = -1 * motor0.state_.vel; // negative sign is flipped - if (curr_vel <= 0){ + //curr_vel = -1 * motor0.state_.vel; // negative sign is flipped + if (motor0.state_.vel <= 0){ #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); #endif motor0.set_velocity(0); - return; } break; case slidersystem::POS_LIM: - curr_vel = -1 * motor0.state_.vel; // negative sign is flipped - if (curr_vel >= 0){ + //curr_vel = -1 * motor0.state_.vel; // negative sign is flipped + if (motor0.state_.vel >= 0){ #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); #endif motor0.set_velocity(0); - return; } break; } @@ -191,14 +186,12 @@ int main(void) { } // Limit switch check - //if (read_switch(motor0.limit_switch_pin_neg, &neg_lim_switch_flag)) { if (neg_lim_switch_flag) { motor0.set_velocity(0); motor0.move_at_target_velocity(); motor0.state_.system_status = slidersystem::NEG_LIM; neg_lim_switch_flag = false; } - //if (read_switch(motor0.limit_switch_pin_pos, &pos_lim_switch_flag)) { if (pos_lim_switch_flag) { motor0.set_velocity(0); motor0.move_at_target_velocity(); @@ -216,7 +209,7 @@ int main(void) { ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif eth.send_packet(&motor0.state_); - return; + return 255; //Delay_ms(5000); } } diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 0629753..ba51db4 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -190,6 +190,7 @@ void ClearPathMC::set_standby() { void ClearPathMC::move_at_target_velocity() { /* Move the motor at the set target velocity */ + double current_motor_velocity = -1 * state_.vel; // Check motor status check_for_faults(); @@ -198,7 +199,7 @@ void ClearPathMC::move_at_target_velocity() { // new velocity is greater or less than the previously commanded velocity // If greater, Input A begins the quadrature. If less, Input B begins the // quadrature. - int32_t curr_velocity_rounded = round(state_.vel / velocity_resolution); + int32_t curr_velocity_rounded = round(current_motor_velocity / velocity_resolution); int32_t target_velocity_rounded = round(target_velocity / velocity_resolution); int32_t velocity_difference = labs(target_velocity_rounded - curr_velocity_rounded); @@ -212,7 +213,7 @@ void ClearPathMC::move_at_target_velocity() { if (e_stop_flag || neg_lim_switch_flag || pos_lim_switch_flag) { target_velocity = 0; } - if (target_velocity > state_.vel) { + if (target_velocity > current_motor_velocity) { // Toggle Input A to begin the quadrature signal motor.MotorInAState(true); // Command a 5 microsecond delay to ensure proper signal timing @@ -237,7 +238,7 @@ void ClearPathMC::move_at_target_velocity() { } // Update the current velocity - state_.vel = target_velocity; + state_.vel = -1 * target_velocity; // Wait for High-Level Feedback (HLFB) to assert (signaling if the motor has reached // its target velocity) diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 6616ea4..eb6dc90 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -146,9 +146,9 @@ void EthUDP::construct_data_msg(slidersystem::DataInterface* state) { sprintf(data_buf, "%f", state->vel * -1); // x direction flipped in ros2 --> TODO: move all of the negative signs into one place! This shouldn't be here. // Create c-str msg - strcat(msg_buf, "\{\"status\":"); // TODO: For some reason status_header gets set to 0. Needs a debugger. + strcat(msg_buf, status_header); // TODO: For some reason status_header gets set to 0. Needs a debugger. strcat(msg_buf, status_buf); - strcat(msg_buf, ","); + strcat(msg_buf, m_delimiter); strcat(msg_buf, data_header); strcat(msg_buf, data_buf); strcat(msg_buf, footer); From af60e15dece42d93cd6cda04e19f1a5bffd0b455 Mon Sep 17 00:00:00 2001 From: lukestroh Date: Mon, 22 Jul 2024 14:33:32 -0700 Subject: [PATCH 13/21] fixed some structs --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 97280 -> 101376 bytes include/system.h | 2 +- src/ClearPathMC.cpp | 3 ++- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 536d38a7d837c4cd33867304a58dbafb5cb435b0..a5cb0998191c072afb64bcb6273043c34c724b8b 100644 GIT binary patch delta 2430 zcmbuAeN0nV6u^6KU!jHK+Jb`eUrE8R1~tY>?06y!)4LTxh;#CI#;8_2s^j5#5uP=wwwHV&OP_s z^X|Rp+*{{;m9sryZ+Z+-_?k|kh(=Y3jEs(sGAq+#KCn$Ds*OLT{P-$;nOszdpv{C( zXJj(1R~~LuV`nZBjNrUc=AcIwAO=K>1R(K<1z|q^_7wJ4_-v^tW09$d5lKRDO^P^V zI`R-FC7@)Bn}9MA!G#b-3f<2|B&1_E3Sprqp$tSOBjLz= zeWtIUkCMe-h;k89fGkD|ktGQ8Uy8CAvHAMUHpOL|mK=eGtw=dy_YGE{tVF7iYGgIS zCeXbLjQUz%X9LQO$Yz%<6h>TCp^%9h=R^}vUgA!rJ@M!R-)v;Z&Z4P&lBIwx|4Aue ze*P42=og8wt@~3j5%njfAQbKYkb)q84#rZD=ug3HoZ?Rb`|P7BVExIM23r_9v5vp_ zQxN2{|K}8>VSxXgf*G6>!?=Sig!U*is6)GqHcU*0MtWC!kz-|rNBH91Q&6j;;JP^}5XY$9e6aSjpliQ8-^l6|2F(2Q3I_F!e`TQS&P z#D(z40hcW;<1JJ=kb{Vb96}BwZzD$#R`$nGzJnY`&bVwvlg^@Y4(UeDBNvb!q?cyo zPIhe^au?nN{nfPz8>j3m8;)`1(D%1z8Wi;4P?1zv&;@*xQ8HE@0uAKiq&$S3N-$ej z7lMs#$xw3>_$9~nhTFG3x3vDY;*Tv?;X7l$!8`c%l1!k6Z?mOrYhSRKNkltwm)k)X zy{7VdY{MbLp&Z4`OFJ(#(ArfzpSML{ZgkA)KlN_&7ag@D-#naS_XgRQR|Dc|;;t(o z9D7;E3c+FJ*xhAyDR~@P;M?6Hx)z977vRn(gc8$yje?ctUyCKSM+NL{WDB64SS!a~3f2i&iAviH zy0x)TU4-fsW@iV-a)df-(&$Y5fRj94a@3RDuVznE9OB)l=M}tAY+Ku`YW9B3@C`_w zZCVhU2;bDlvA>Kl=cYl9JuKeO?M9^&!A3{-@1V}`0&cvBJ>gj(^d~|Ty=^lHCjiuf zIU6`SAv2yHsEwr^6C}jX<`SvwlViZQIVESG6NGEC zq55wV#qaUtBD`}o?kn{~s$bX);x#U)4R^qfDnLO8Zlv)atrIF1ftu#tGD*jNNm0^+ zt}<_UYVDp%FaCUq_S~5JsFyN8k^ksSDfiYbPQSyuPxcem{U!cJNV%R1KwCc^5c`NU za6eJ$dyK~XhEWhCe^97WL5DCB2_ZD;SCf?T=|zy2hrm&yXKC;^RvVFWxeBZ*R)OP< zZ6xgInIPbBzw((0U`|C4(4_CsTlO0g06r2^XoYp@RF0VJh8$i=9TV<;eP6X1* zuX5y^Tv$c~QVoN+>e}baE$i1*)mb*wtx0_WaCz)sa=GxsCNK#P7D2pRng?x4;cgAo z%QqIl>mbZ31-pE9Ex3X1Xw9Kliga>e8FaFs9KBW zVn|vIRquZ4Lj!7W7~`ZS;JwJhPO6s>r!Ec&ihxx5j&3_Nk~tYq%292bYLbN4#P+UGB3C@WP|;7 z#?$+GyZo*ZUEw_V*_&?T-MZl?g#+J8CLlgcg3SdDpcF5fE}y^bznZ&4y+&! zWP_zZ#LS2NJM41Uh2SgTE+FP8gcXZeB}uZB&O>_3y`^ZDfK;#uq<|Gbgc7AJTF-&a z0BOMDb1z8xTC_t9O7yvx7p$4%5Kb4`8>7BWu#KPzG=p#W+!@s2b6cp(=e~R0Vz|U7 zu~LBc6QBp|jP~t<-3`72z8m%R!tMq8z*B%fr*6ZPf0N-(n#T`5DfAX|2t9@UGVKk~ zad*=Go@if8dp+87wHNW1z&A(x#F+R|?TgS|4C29D?ZvhUt%deNcM(dI|A+SV|10gq z0)vS)GEc~>xyRh7m$7o3_B>+#llfiVx5%bWC)s4SW-gx{li83!4h#~fT@2*u%!b*X zWeo-r$4Pu-K84DAKoq~Z=0C*^YQEfTzH!BL`e)xdUiy80t*DA+TNO{} zAhylL&dz!@xnN2k*enOKW!_(ISM6o*P@q&a0~PYUVte)@=+mGNJOiEu-vbAL=syVi z9Owr>05iTe%Zj6@90SLJ0tUbda1uNZ2Eh>ck+;T~Mq<6ap^*GJ>-05Z_K3(i{iLKWqf)ih^`WRdkrihl*G^ z3WvMa8o!RpGSP^{C~P3H9j0PFxPxTh##A-6ev;>f&$r zHt5AYFH(c2dtF9x(!WRpHyX|iq(N;YJ;Xy02{>(Aq9%K%5aY}8r`9NLu2}k*=A@7E zVF`8cyv;d$syLflR@M6KEealT#F;PYX5T`>lgDjG7h(gA_0leBy@ZE^ ze}CMnetdqB&Og{fhxmcP7ub3_kI$W2h)w1PPd6pl@j!}eTH>se86+uB)zLlNebVe7 z4h4t2q=tqa#C=*m=a;1g24vMy<|VGwDtZ5K4xcHD<8wRH`Cv^E|FNus?OKrAcV%Kl z5>t5r+R4V2>xm+~#ExA>2B-9}G=?Nx<3H%-7K@XemRGse;#A*wu0n5e;__aAp?c{W z)0F|9yjje@_;?kcx%wzSI%(%~A9>hqaj1^7p#*jHrFxo=pT>(kad_x~8xQt4zW7R~ zmz;CIRJZ=&r}`9;iLvrSRCH?P^y}omzDIp&>Ms-+l-2E@9iWt7#m2zC1?H>nndJF+9G zyMJnOI&|Q~%SSgVwM*%7Wk)?VE2B0_Q$}^@c>N#2>;JG+!&hpm$aaw^tYcScNDHNsgT_`;vtGNM zO)pYlSQcBW=8tZW?>K=v7~EnA{5FnPA1l;CIrKIKCXI^qKFVQFN3!;ME>+U8ThycU zRMKK@yCIbH>eSEv7N>vVP$lWLHr&dj)fwobndx|uVy&Rcu^Ku=Of$ui!P=OGYUBK^ zp=7hE*fj6?uBN`uEb~v|ua@vTEqj!KPWr0SwT(<1YW6DjEOKcdIB7KgY#tP9)lr%D zW<52^+-T{>IocAXj2%bxhC=AktbF(c Date: Mon, 22 Jul 2024 16:11:30 -0700 Subject: [PATCH 14/21] removing some bugs from the system_status structs --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 101376 -> 97280 bytes include/EthUDP.h | 2 +- include/clearpathmc.h | 7 ++--- main.cpp | 32 ++++++++++++++++----- src/ClearPathMC.cpp | 8 ++++-- src/EthUDP.cpp | 12 ++------ 6 files changed, 37 insertions(+), 24 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index a5cb0998191c072afb64bcb6273043c34c724b8b..4afd37330c2b80e155cee23408dbad9637c122e3 100644 GIT binary patch delta 1859 zcmcJQYfM~46vyw(-dz@0D&;NEWrgyF3J8=^QYed8J15HXo!T*6>B{BAciIe>HoO@=@ znVJ8cJKHm4>A7n@P!zsJ(=-!UT3%j;S@B~>Yh$=&qOY)K-_NR*f6BU}(31!P0gViT zg;}oj+GtomVD~<6ge?UY@Br8iLO~%|4>o{%$WMcP7$ks40M*Qar8&Z213-$VRcV=U zl0gPY2XP=5H~=MCVdFtCNCkNyA7p_@Kw~0c!$FL@HZ-Wf8>Eh6cWpvm^_l<$Yw%p> zjXeQd51s^1fv3SU;5k%cFSu(XS(3YUbHRfMy$oES4N#d^U|$8Vfp+jZ=m4Fd3%miu zBHL}Q|0%i;YFp&0+LY^SYgMPWK&tD2R5t@s{UP8}o$}VIz6pKzQ=J5(9IxtiKH2Z9 zdJK|mAOw)=RN&uKUk8c)55%k$iiO0A)xM-^;Y`XF^GP*UD)$(AMNd?Ua3v2IVZPG+^{YiAGEBmel;F$sXLSDl zN|{Hkm6aq{n^`#(!b$=s;amo2o?~$=-E%vW<*J5Q$&7dhR}RN4GlHj>m0q<2(Ch)D za8A)l*VIx1=dzp+raApt6apj`jZv<&D@MT()r8&Se*u9o)#6Vrr^aH+?+a_xD!JD_c)wcObHh867oxa^;ilF~7 zU!zw0ZtTCPaHilI)n%hjb*;e)8_r?AvLKqh} luE(q4Yb8vT+?>!w*2M*}Fn2`!ay>x|UrUsY{Dhq~{0VMP9mN0u delta 2152 zcmbuA4@}f$7{~8=73I*{BtRCnlt}^$e0-#5gtXZH~O z*&rWG1L;ZvY#wLg3neg0feKVo4EyO|y8>FRxBz+~SOgXW z2Ur4B{8H#j;0(5v&r@a1PegkgSO;o?ee0oX!3IzV>Vb;qoh?>NW3cfmbQ5UtIOFJ{ zXG0v#gr&LR^0F=Ag(nIEw;!%ko4R>5J}jtJ7Sw(pmjx9U%7TWtB%sb3%0fKs$7LZ7 z{wHL?5lX|qW??oa31vZTz&ncdXJe=~olsiNZUq#QXCZDzjALE?kPLi1sGkEv5ayk`3o#)RERw@KcQbFfHK z$5zFpCgZMzWqg1Y$&rM7qqLS>C!wcPp13=mie=3toxI7JJag$(O_lQ4fZ8fP$O`#L z?XpO+!E67!XG37BaounX1A{?6OlQoOW@XZvb)CGi*j_A-6dQ=+Be7K`_4%rTADcR^ zLr-Jg)(VrV8I{Jsd0ja}B<N7O2qk<2S-8# z;0!nm`oRDg1o-@Z)fZ^K@^hrkM_U~{N-g;XfT-QZR@@=IGDhFR^kU|l-$-%B6>VQ$ zxN>3j#;yH*t=_cFeGu}#`K`y+Tymr1xYC`KlC z-6CX~yR&NoISy|2xGzHgkjaa;3yAl8)xaC>6w0srW}EbGIDgMIiqA}Bmb1=(Gal3W zFPh0l(sqqU@H@9J%ifOs~=s(B~E?&xa~xly!l|8W>J63kUXOdk)J{x;%X(8M|$xR%*Fe+QWQBT zo>xwa@n@yc57pdrK2LOQrd(lJMK6oj>M2pIZ>5Fe))v|!x;D`!apZL>^;;Y11EVru zr-PCa%9k_5796_5QUjR7s+9C24$5 T*}B8@29YS(N-O motor0.state_.vel) { motor0.set_velocity(motor0.state_.vel + 1); } @@ -209,8 +224,8 @@ int main(void) { ConnectorUsb.SendLine("EMERGENCY STOP TRIGGERED. CHECK ALL HARDWARE."); #endif eth.send_packet(&motor0.state_); - return 255; - //Delay_ms(5000); + //return 255; + Delay_ms(5000); } } @@ -220,8 +235,11 @@ int main(void) { else if (!poll_switch(&motor0.limit_switch_pin_pos)) { motor0.state_.system_status = slidersystem::POS_LIM; } - else { - motor0.state_.system_status = slidersystem::SYSTEM_OK; + + // Poll E-stop so user knows to properly reset the switch + if (poll_switch(&motor0.emergency_stop_pin)) { + motor0.state_.system_status = slidersystem::E_STOP; + e_stop_flag = true; } // Move to target velocity (blocking) diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 1abacf2..5767e83 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -16,14 +16,16 @@ extern volatile bool neg_lim_switch_flag; extern volatile bool pos_lim_switch_flag; extern volatile bool e_stop_flag; -ClearPathMC::ClearPathMC() {} +ClearPathMC::ClearPathMC() { + state_.system_status = slidersystem::SYSTEM_STANDBY; + command_.system_status = slidersystem::SYSTEM_STANDBY; +} ClearPathMC::ClearPathMC(int _id): motor_id(_id) { - state_.system_status = slidersystem::SYSTEM_STANDBY; - command_.system_status = slidersystem::SYSTEM_STANDBY; + ClearPathMC(); } diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index eb6dc90..2ceee68 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -107,25 +107,19 @@ void EthUDP::read_packet(slidersystem::DataInterface* command_interface) { if (packet_size > 0) { udp.PacketRead(received_packet, MAX_PACKET_LENGTH); new_data = true; - + // Parse data from the received packet // Extract first field char* received_packet_cstr = reinterpret_cast(received_packet); m_token = strtok(received_packet_cstr, m_delimiter); + if (m_token != NULL) { - //#ifdef __SERIAL_DEBUG__ - //ConnectorUsb.SendLine(m_token); - //#endif command_interface->system_status = static_cast(atoi(m_token)); // Extract second field m_token = strtok(NULL, m_delimiter); //token_cstr = reinterpret_cast(token); if (m_token != NULL) { - //#ifdef __SERIAL_DEBUG__ - //ConnectorUsb.SendLine("got far"); - //ConnectorUsb.SendLine(m_token); - //#endif command_interface->vel = atof(m_token); } } @@ -154,7 +148,7 @@ void EthUDP::construct_data_msg(slidersystem::DataInterface* state) { strcat(msg_buf, footer); #ifdef __SERIAL_DEBUG__ - ConnectorUsb.SendLine(msg_buf); + //ConnectorUsb.SendLine(msg_buf); //ConnectorUsb.SendLine(motor0.command_.system_status); //ConnectorUsb.SendLine(motor0.command_.vel); //ConnectorUsb.SendLine(motor0.state_.vel); From a1dd7317b102bd9ba056fb357f411a96b00a3bcd Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 25 Jul 2024 14:09:33 -0700 Subject: [PATCH 15/21] many debug steps --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 97280 -> 102912 bytes include/EthUDP.h | 19 +++++--- main.cpp | 8 ++- src/ClearPathMC.cpp | 20 +++++--- src/EthUDP.cpp | 54 ++++++++++++--------- 5 files changed, 56 insertions(+), 45 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 4afd37330c2b80e155cee23408dbad9637c122e3..1648b3142dcce70317cadfcf681cf3805f4deb48 100644 GIT binary patch delta 2348 zcmcJQdr;I>6vuz(?r-I_;4a8RR9NuAk_w9gsUad_CE&_xz5r2?e4_GLd^XB}LxzsB zd#me^`S^(%=4kA7+{r;jKx3S0kktGqB~5L#NNb%!6FPSVYkK`vcjhzq+;h)8_xJnW zSLrY6(waV13I0KnB&m?D?(S}^T=vj9^8ljGsC_cU&I`$husIw%q7W~{4e>_AfE28C zh!4^S@kh|z-4E+X#2x92WFcB)AYw!YA;HKCh=?DI^++TD>5quqW3YZII=j0~-SOCn zKt>?Lkx*n55{pbk#vw_FCz6TyAqHeL5{-x~1A6K>t2s~`h7C=RLv&JYNsunR$1xRa z6QZ}8qYZ&*3_)@b5jh|01;|1q7s*3h8LCPQ9;>lYfRtFxez4hUj*v^SWh43O_AEKo zP76snsQIHlq0clG<3#DzR*k_zTjBeowleyJwnCSG(l!M9d$kqgg|V5pv=tMI@?2YAO#ICC|7qL56ekgq2vi_RGU94;f6pjFn^ZCCu^94%uGegY9rTgr z3t9XyEe$eor}wmfkDyLn6b+A>PAv5ia}P!5L;>rZLTzO zOX@W6a2@$Dj1ep5}C`fnV{{w`S8Nxc}F*2Z%v;YX!A*=tKw|1 z)oKofF6+WT-A)`^g@`|}A$#zg#_UC7AF>}&kON2!au7L$97aAsY7wzXPFu}sk!P@R z7HL4vA&p2A(u|x(E+C&EIPEQ4PAP1m=Q1`9D8WNtfEVH-scLcr-CO2tEqrO$GV8@T z!-do=(F_4vjaR&v&wDFpZdnngIqi1-MVhhJ&Phg>7aJ7@IW(swSl3bAx%kVl?EL-p zA8#mY&@XeX)9#i$u=!3Bo0nUHw}UI3=Oim&tH5<3Mrx_ZaM$BEg(G5BxU*seaD84o zuw;M`9p?cQ`>wL2>f11%4(z618{Kq%~<4LkVU-HQl*D9zkE zi`Uns5M_t_d>1K)e0rp4QGk`@f{B|>*i~L__-=^X-c9<9!r5I+8vyZi(7j9a(*0<< z?|g((Pw2yjG@AH>fp*-xLf3tJmDb$7j3N8k`!cw*K{6Qn;u(^N6f^HfTTPiI>Gw>$ zx?_P_X(9CSPos4u7}Q%T(eUMo&U$sgv0fk6;=vOyYRYBtF*NDacz*lw23aT8s;4xF zH!H|??VxLV2To6AU7y}w(TAwG`NnaX8$VS8L^(oyV2eum#@%7oKm+K+yVYBUp%KN- zxkEH9bS~%1JKohXvk$DHJMYIjsy$$qikUsY|M^_Eyd4w!K^U-)+(Ba$$5Oo0;;B?2 zZph^0McxJ5A9T=u@ma^dD?04)}tzJ^JaHam`W%;iQx#N&o?CWAM zIASuO6&x@Jx(BnmwJ?^2E`|Y)*QUc!H7i&FGnj5REMtXv5Xk1QgICy{d?;ttOJOOi zT>}XY{VI4{CCoY0YfL;lvmUf8vk0!R)hqDY%`65#RY_g}D=dbGq#0Pu42WgdSHXDJ lz6A2<%7pojtP(H*tD1>FwQYR%VOR^mqKjds1E-s+`VCR#yBh!i delta 2026 zcmcK5ZBSHI7zglu&fbf=un;VuNe#bKKCLo@BnZ=Q3{ zx#zj(KKIve9=6FdJmf0z`R@6vSA_HW(@m zGtkI_TzCQ|Lm@aoPBI}nArW$54$OsFFahK_;}KKA=B-YSFN$``ff8?Z#+<4#9kO#V zj%%WQFCf;!i%#)NV?N6kJXp{f#ZWd=Rqm3dq8%APYYQMhh?JjV*j0#vWC8Su;5& zTKJmL%0II3HcYaB6=dOMf`=780af}xieCI*i!K-IPjE5b&+=9I65Qk4EIWwv-z?Yo z?$lfnim!U;AP*(wac5Qy_qc171(?j6)3SNUU1cVjcj0J{XIWJwe;+!oh;VWi{&Fr z<(4*xp$x|S*>@@N*4oU*^b0Jd@F*KVgBRTSJ8~qC^NRb;yUSDlO;~d|% zu+^7NOt&*_@FN8a{(FAh(_$Q9uuT4gmht$v%`_ z^`a3F;gWGoE65_ZaM$bPt+5L0^t&X>lJw8A7_^hqXpwmCnE?5p?kC=F%va|k$sr!z zcU6(!rjf_la$QM|9>mIfm8-_s(Aw@K>-~FQxsLw4^w-U|=%!=9YQUF3Tv=B@nl+P( zMA72Qary?O-RhFbxyM>%Hk4wnOU{pM73+Yh90wKAj<_x~ZUBCK`1OlwQ+=N^lJrJH zHtDO_gB#l1L-E_s&u!jwY}aAs<2wxpd09@0Rz8{R!oKD^($_O>?+U6El>tBLjjX-m zk`gDAMz(YX%2&YrO^k6?SY|dnj}iF-$u$X2VH@dA=3n5Um{-u*k;S(!eAY~I%cMt7 z;-NWJ!nOMgV|z=55?MU*a!*U`zn6`N_NRZC*vv%Y0W0H9yYmrqQlghoBc&GMR&#=O zXCBq_gbh=9SA&z+k4qE1hf7KCj?vo`y^E2;$yOpteZBDy?%5dIf&U5l*kTNP@TZyW zed~B}|156ry)L%?@G{fOnHc zF_A?LV~B~Qfm)R>8%_;=X`-jh+65yy^#BVFTBwwQD^jUf(eIczg~OW7Mx7eVqIG=A z#mExG2YvOC3yB$_YjHk1R)2CJRqIs96r7~c_wha_F<%PB2J%QZYr2cvzHAyFsGwhx zCTv*ws%!n~ffSURa^i7 diff --git a/include/EthUDP.h b/include/EthUDP.h index a68cd52..386bcec 100644 --- a/include/EthUDP.h +++ b/include/EthUDP.h @@ -28,17 +28,20 @@ class EthUDP { const uint8_t MAX_PACKET_LENGTH = 128; // Maximum number of characters to receive from an incoming packet // Send data attributes - char status_buf[2]; - char data_buf[10]; - const char* status_header = "{\"status\":"; - const char* data_header = "\"servo_rpm\":"; - const char* footer = "}"; + char m_status_buf[2]; + char m_data_buf[10]; + const char* m_msg_status_header = "{\"status\":"; + const char* m_msg_data_header = "\"servo_rpm\":"; + const char* m_msg_footer = "}"; + + // Data buffers + unsigned char m_received_packet[128]; // Buffer for holding received packets + char m_msg_buf[128]; // Send message buffer public: - // Data buffer + // New data flag bool new_data = false; - unsigned char received_packet[128]; // Buffer for holding received packets - char msg_buf[128]; // Send message buffer + // Ethernet UDP EthernetUdp udp; diff --git a/main.cpp b/main.cpp index 30fe409..f6fa5ae 100644 --- a/main.cpp +++ b/main.cpp @@ -107,15 +107,13 @@ bool poll_switch(DigitalIn* switch_pin) { int main(void) { -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ set_up_serial(); #endif // Set static address for the ClearCore Controller - //IpAddress local_ip = IpAddress(169, 254, 97, 177); IpAddress local_ip(169, 254, 97, 177); // Set remote (host) computer address - //IpAddress remote_ip = IpAddress(169, 254, 57, 209); IpAddress remote_ip(169, 254, 57, 209); EthUDP eth(local_ip, remote_ip); @@ -232,11 +230,11 @@ int main(void) { if (!poll_switch(&motor0.limit_switch_pin_neg)) { // This is now working as it should?? Can it be? motor0.state_.system_status = slidersystem::NEG_LIM; } - else if (!poll_switch(&motor0.limit_switch_pin_pos)) { + if (!poll_switch(&motor0.limit_switch_pin_pos)) { motor0.state_.system_status = slidersystem::POS_LIM; } - // Poll E-stop so user knows to properly reset the switch + // Poll E-stop so user knows to properly reset the switch TODO: move all e-stop stuff to interrupt if (poll_switch(&motor0.emergency_stop_pin)) { motor0.state_.system_status = slidersystem::E_STOP; e_stop_flag = true; diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 5767e83..8de67c8 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -9,6 +9,10 @@ #define __SERIAL_DEBUG__ 0 #endif +#ifndef __CPMC_DEBUG__ +#define __CPMC_DEBUG__ 0 +#endif + #include "ClearPathMC.h" #include "interrupts.h" @@ -50,7 +54,7 @@ void ClearPathMC::begin() { // Enable the motor motor.EnableRequest(true); -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Motor enabled."); #endif // Enable pin interrupts TODO: This is totally in the wrong place, there should be a system manager file, would also clean up main.cpp @@ -73,7 +77,7 @@ bool ClearPathMC::check_for_faults() { if (motor.StatusReg().bit.MotorInFault) { if (HANDLE_MOTOR_FAULTS) { handle_motor_faults(); -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Motor fault detected. Move canceled."); } else { @@ -91,7 +95,7 @@ void ClearPathMC::handle_motor_faults() { * Assumes motor is in fault * (this function is called when motor.StatusReg.MotorInFault == true) */ -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Handling fault: clearing faults by cycling enable signal to motor."); #endif motor.EnableRequest(false); @@ -104,7 +108,7 @@ void ClearPathMC::handle_motor_faults() { void ClearPathMC::assert_HLFB() { /* Make sure the HLFB is connected */ while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED && !motor.StatusReg().bit.MotorInFault) { -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("ERROR IN HLFB ASSERT:"); ConnectorUsb.Send("\tHLFB STATE: "); ConnectorUsb.SendLine(motor.HlfbState()); @@ -151,14 +155,14 @@ void ClearPathMC::set_velocity(int vel) { // check the limit switch statuses if (vel <= 0 && state_.system_status==slidersystem::NEG_LIM){ - #if __SERIAL_DEBUG__ + #if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); #endif target_velocity = 0; return; } if (vel >= 0 && state_.system_status==slidersystem::POS_LIM){ - #if __SERIAL_DEBUG__ + #if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); #endif target_velocity = 0; @@ -175,7 +179,7 @@ void ClearPathMC::set_velocity(int vel) { else { target_velocity = -1 * vel; } -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.Send("commanded vel: "); ConnectorUsb.SendLine(vel); ConnectorUsb.Send("target vel: "); @@ -253,7 +257,7 @@ void ClearPathMC::move_at_target_velocity() { // Check to see if motor faulted during move if (check_for_faults()) { -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Motion may not have completed as expected. Proceed with caution."); } else { diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index 2ceee68..52158ff 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -9,6 +9,10 @@ #define __SERIAL_DEBUG__ 0 #endif +#ifndef __ETHUDP_DEBUG__ +#define __ETHUDP_DEBUG__ 0 +#endif + #include "EthUDP.h" EthUDP::EthUDP(): @@ -65,7 +69,7 @@ void EthUDP::begin(void) { // Check physical Ethernet link while (!EthernetMgr.PhyLinkActive()) { -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ ConnectorUsb.SendLine("Could not detect a physical Ethernet connection."); #endif Delay_ms(1000); @@ -76,14 +80,14 @@ void EthUDP::begin(void) { if (m_using_dhcp) { bool dhcp_success = EthernetMgr.DhcpBegin(); if (dhcp_success) { -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ ConnectorUsb.Send("DHCP successfully assigned an IP address: "); ConnectorUsb.SendLine(EthernetMgr.LocalIp().StringValue()); #endif } else { while (true) { -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ ConnectorUsb.SendLine("DHCP configuration was unsuccessful."); #endif Delay_ms(10000); @@ -105,12 +109,12 @@ void EthUDP::read_packet(slidersystem::DataInterface* command_interface) { /* Look for a received packet, store in 'received_packet' if present */ uint16_t packet_size = udp.PacketParse(); if (packet_size > 0) { - udp.PacketRead(received_packet, MAX_PACKET_LENGTH); + udp.PacketRead(m_received_packet, MAX_PACKET_LENGTH); new_data = true; // Parse data from the received packet // Extract first field - char* received_packet_cstr = reinterpret_cast(received_packet); + char* received_packet_cstr = reinterpret_cast(m_received_packet); m_token = strtok(received_packet_cstr, m_delimiter); @@ -131,35 +135,37 @@ void EthUDP::construct_data_msg(slidersystem::DataInterface* state) { https://stackoverflow.com/questions/23966080/sending-struct-over-udp-c */ // Reset buffers - memset(&msg_buf[0], 0, sizeof(msg_buf)); - memset(&status_buf[0], 0, sizeof(status_buf)); - memset(&data_buf[0], 0, sizeof(data_buf)); + memset(&m_msg_buf[0], 0, sizeof(m_msg_buf)); + memset(&m_status_buf[0], 0, sizeof(m_status_buf)); + memset(&m_data_buf[0], 0, sizeof(m_data_buf)); // Set data - sprintf(status_buf, "%d", state->system_status); - sprintf(data_buf, "%f", state->vel * -1); // x direction flipped in ros2 --> TODO: move all of the negative signs into one place! This shouldn't be here. + sprintf(m_status_buf, "%d", state->system_status); + sprintf(m_data_buf, "%f", state->vel); // Create c-str msg - strcat(msg_buf, status_header); // TODO: For some reason status_header gets set to 0. Needs a debugger. - strcat(msg_buf, status_buf); - strcat(msg_buf, m_delimiter); - strcat(msg_buf, data_header); - strcat(msg_buf, data_buf); - strcat(msg_buf, footer); + strcat(m_msg_buf, m_msg_status_header); // TODO: For some reason status_header gets set to 0. Needs a debugger. + strcat(m_msg_buf, m_status_buf); + strcat(m_msg_buf, m_delimiter); + strcat(m_msg_buf, m_msg_data_header); + strcat(m_msg_buf, m_data_buf); + strcat(m_msg_buf, m_msg_footer); - #ifdef __SERIAL_DEBUG__ - //ConnectorUsb.SendLine(msg_buf); - //ConnectorUsb.SendLine(motor0.command_.system_status); - //ConnectorUsb.SendLine(motor0.command_.vel); - //ConnectorUsb.SendLine(motor0.state_.vel); - #endif +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ + ConnectorUsb.Send("Constructed msg: "); + ConnectorUsb.SendLine(m_msg_buf); +#endif } void EthUDP::send_packet(slidersystem::DataInterface* state) { /* Send a packet */ construct_data_msg(state); +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ + ConnectorUsb.Send("Sending msg: "); + ConnectorUsb.SendLine(m_msg_buf); +#endif udp.Connect(m_remote_ip, m_remote_port); - udp.PacketWrite(msg_buf); + udp.PacketWrite(m_msg_buf); udp.PacketSend(); -} \ No newline at end of file +} \ No newline at end of file From e1615d50eeae701d012122018d547fb5d2b985ff Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 25 Jul 2024 16:13:48 -0700 Subject: [PATCH 16/21] edited debug flags --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 102912 -> 102912 bytes main.cpp | 12 ++++++++++-- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 1648b3142dcce70317cadfcf681cf3805f4deb48..588f786690b08dea0e01c93cbeeaf8c2aaf7e027 100644 GIT binary patch delta 453 zcmZoz!q%{aZ9@(d+k_tv;-21{T*TDG$hi3{lQ*N`e;^QLU|;~zB0yRbh&h0mABdxY zSO$pAf!F|uWr3J&av_U63rO|kDJ&kmQa~X~AeIJV(}{uNlUQ>YEhgu({%31A*RcKR zz0E~z=NJwD0hNO+0jVO!5*8FoSV5KuO@7Ft%qTFKl{Fn?0q1lUCPoH^#T;BLlR3C} zHjD7-^GtSD+se3U^G`K)#ziu0lhl4NZkqg4t(uh)sPNF_Ku_<9zzC$?r|p(`4|DtrUR#oO=oSdWSAHz zJekS=-y#j6O-f%Fxj_E<4|E&@3=2>p$oPMFjF$t-!;A-+rv>Co0kJX=>jJSD5K96v$P6_g{TE0}1L?_gSR8mk z8eqV5VxagWRu)E~$*ipDAjfb{pUlE1uvvub3-e?VE`f;!JeyVcWT~x!gNQG&V)DOl(n}15NGcM8)nq=oP`ACPs p<`TOe#zi(Ji>jD_Xi|XK0|1=gahU)B diff --git a/main.cpp b/main.cpp index f6fa5ae..371dde0 100644 --- a/main.cpp +++ b/main.cpp @@ -60,6 +60,12 @@ #include "ClearPathMC.h" #include "system.h" +#if __ETHUDP_DEBUG__ || __CPMC_DEBUG__ +#define __SET_UP_SERIAL__ 1 +#else +#define __SET_UP_SERIAL__ 0 +#endif + // System state variables volatile bool neg_lim_switch_flag = false; @@ -67,7 +73,9 @@ volatile bool pos_lim_switch_flag = false; volatile bool e_stop_flag = false; constexpr uint8_t DEBOUNCE_TIME = 10; -#if __SERIAL_DEBUG__ + + +#if __SERIAL_DEBUG__ || __SET_UP_SERIAL__ void set_up_serial(void) { /* Set up Serial communication with computer for debugging */ ConnectorUsb.Mode(Connector::USB_CDC); @@ -107,7 +115,7 @@ bool poll_switch(DigitalIn* switch_pin) { int main(void) { -#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ +#if __SERIAL_DEBUG__ || __SET_UP_SERIAL__ set_up_serial(); #endif From 570354e1cb2f1970e9a6264373c8fdcedcfc0a72 Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 25 Jul 2024 16:26:37 -0700 Subject: [PATCH 17/21] minor edits --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 102912 -> 102912 bytes src/ClearPathMC.cpp | 2 +- src/EthUDP.cpp | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 588f786690b08dea0e01c93cbeeaf8c2aaf7e027..cb56caf1c569958293aad236d22838274f6f3c95 100644 GIT binary patch delta 556 zcmZoz!q%{aZ9@(dtAVYg*vrXW2H$P&$<7@OoD4od3;aNqG)QPn{vzei$q2IFA1cDl2sHN) zkOSiXmsFm-NFj)u6(Kf#p(>;H^bg96CY$4Aj<8NviVtGt1xl})e6iPivPu Zx3T<;+{?}WNKz)-29cPoKZlQfq{Vyh_!(DKM+i=;gFd;gT;f> z5-1=I#HJGi#W%6$FtRqBYuFCa%DVY0TQDOFNOrOhivrjT79a=R3`U{J4_TBM1vayC z@-t30;S-pw!X+><#bk2^*BoZve?Z%q85tN@fcW3UL}9oj-)-*6hnWl~mvn@%_<`(b zkkFX?MXiCA38-?_`;1Ok%qwZi~Wp_lN~MoGj5ygs5y7~1a(HK%|9$TSQZs9Epp(RY{0~_okfN*ic#R_ rqmReZV%OB)yWkTkK8eq6dY&xfq(weVld2jvH Date: Thu, 25 Jul 2024 19:28:25 -0700 Subject: [PATCH 18/21] messed around with debug print flags some more --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 102912 -> 102912 bytes include/EthUDP.h | 4 ++++ include/clearpathmc.h | 4 ++++ main.cpp | 10 +++++----- src/ClearPathMC.cpp | 16 ++++++---------- src/EthUDP.cpp | 10 +++------- 6 files changed, 22 insertions(+), 22 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index cb56caf1c569958293aad236d22838274f6f3c95..baae79e03dcdd7009c69e0168bc5786c53bc354a 100644 GIT binary patch delta 1060 zcmc(d%TE(g6vpqJcIwoY0xe*n(Aw4l1>~VniYT@L7leXEP>D7ONgz3k!ZrhZ)$Pn&V_gK%XjA7bM86!%!DTu z;Yr1%Qg;7>a=c1M>4QcS$GdtCto>lexHHrwe&?#`sZ>Hp9azIsLsy~c&;pPK>_7m? z;4sJmm7o-40x3@cH2^Ev3NpZUAf2Uyu0XS(4v+_mKngGs!tTj)QgdPFgDs#06p9}O zD~%}n_=Rl^OMlu`v{1R!7W2sU%4!(#9$3F%Ft1mWy7_B0c1LbCuh*$c)RCf%u%#X= zK;d|^Zbs>%yHY73TA%~*fj~17wjP?9uuGuXAO{!|zEyE_<6~L92CvBp*FPSoxup^} z8$Z!#%ti~M3szb#wr9WKX$3QDTX+MCEVE{7gZR?2CR0)mF46>8Sf9@pvs&4_tcZmS zUZD|AePTmg%u}oBP3Eq-Xm*ezq?7pZ43c6JAU(5HhirT7$1k6_(f7sqb%R-PN*nh*e(<2WDEj$m6qvR7$J^j{dpM%@zxtS69T zh>S#$4kUosMqOC{+79^Q_*amV$N+uklzoTs*N?=IK_p14P6Z^3=kEZY|E`VgF6Oni zg{uHwdQl9y8tD-_BZk6_9NXW{v3J==j#YKaP;_gsQJPy&_+>D!p~bu4&V!7RlH{G^b=-U`OeQ9cR@PNh(e!ML!Dmx&Ou7^m8!dg7{-q zFCU8j-k@3k2=nD3%KqeFk}a;l6#IM^Zm|3kI9YiXZnDWo5HJ(c#}N&@k|*wIe>F3{ zd_OFGIgc`Q8S aZgW|VlJkky8GUihoF{1hWZ~@s`2PWGmE-jQ diff --git a/include/EthUDP.h b/include/EthUDP.h index 386bcec..367f78d 100644 --- a/include/EthUDP.h +++ b/include/EthUDP.h @@ -11,6 +11,10 @@ #ifndef ETHUDP_H_ #define ETHUDP_H_ +#ifndef __ETHUDP_DEBUG__ +#define __ETHUDP_DEBUG__ 0 +#endif + class EthUDP { private: // Local IP address, port diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 8b773ff..19f0851 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -14,6 +14,10 @@ #ifndef CLEARPATHMC_H_ #define CLEARPATHMC_H_ +#ifndef __CPMC_DEBUG__ +#define __CPMC_DEBUG__ 0 +#endif + // To enable automatic fault handling, #define HANDLE_MOTOR_FAULTS (1) // To disable automatic fault handling, #define HANDLE_MOTOR_FAULTS (0) #define HANDLE_MOTOR_FAULTS (0) diff --git a/main.cpp b/main.cpp index 371dde0..3cc627a 100644 --- a/main.cpp +++ b/main.cpp @@ -52,7 +52,7 @@ */ #ifndef __SERIAL_DEBUG__ -#define __SERIAL_DEBUG__ 0 +#define __SERIAL_DEBUG__ 1 #endif #include "ClearCore.h" @@ -60,7 +60,7 @@ #include "ClearPathMC.h" #include "system.h" -#if __ETHUDP_DEBUG__ || __CPMC_DEBUG__ +#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ || __CPMC_DEBUG__ #define __SET_UP_SERIAL__ 1 #else #define __SET_UP_SERIAL__ 0 @@ -115,16 +115,16 @@ bool poll_switch(DigitalIn* switch_pin) { int main(void) { -#if __SERIAL_DEBUG__ || __SET_UP_SERIAL__ +#if __SET_UP_SERIAL__ set_up_serial(); #endif // Set static address for the ClearCore Controller - IpAddress local_ip(169, 254, 97, 177); + IpAddress local_ip(169, 254, 57, 177); // Set remote (host) computer address IpAddress remote_ip(169, 254, 57, 209); - EthUDP eth(local_ip, remote_ip); + EthUDP eth(local_ip, 8888, remote_ip, 44644); ClearPathMC motor0(0); diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index d724a79..2d206bc 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -9,10 +9,6 @@ #define __SERIAL_DEBUG__ 0 #endif -#ifndef __CPMC_DEBUG__ -#define __CPMC_DEBUG__ 0 -#endif - #include "ClearPathMC.h" #include "interrupts.h" @@ -64,7 +60,7 @@ void ClearPathMC::begin() { // Wait for HLFB assert_HLFB(); -#if __SERIAL_DEBUG__ +#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Motor setup complete."); #endif } @@ -95,7 +91,7 @@ void ClearPathMC::handle_motor_faults() { * Assumes motor is in fault * (this function is called when motor.StatusReg.MotorInFault == true) */ -#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ +#if __SERIAL_DEBUG__ // || __CPMC_DEBUG__ ConnectorUsb.SendLine("Handling fault: clearing faults by cycling enable signal to motor."); #endif motor.EnableRequest(false); @@ -108,7 +104,7 @@ void ClearPathMC::handle_motor_faults() { void ClearPathMC::assert_HLFB() { /* Make sure the HLFB is connected */ while (motor.HlfbState() != MotorDriver::HLFB_ASSERTED && !motor.StatusReg().bit.MotorInFault) { -#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ +#if __SERIAL_DEBUG__ // || __CPMC_DEBUG__ ConnectorUsb.SendLine("ERROR IN HLFB ASSERT:"); ConnectorUsb.Send("\tHLFB STATE: "); ConnectorUsb.SendLine(motor.HlfbState()); @@ -155,7 +151,7 @@ void ClearPathMC::set_velocity(int vel) { // check the limit switch statuses if (vel <= 0 && state_.system_status==slidersystem::NEG_LIM){ - #if __SERIAL_DEBUG__ || __CPMC_DEBUG__ + #if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); #endif target_velocity = 0; @@ -249,7 +245,7 @@ void ClearPathMC::move_at_target_velocity() { // Wait for High-Level Feedback (HLFB) to assert (signaling if the motor has reached // its target velocity) -#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ +#if __SERIAL_DEBUG__// || __CPMC_DEBUG__ ConnectorUsb.SendLine("Ramping speed, waiting for HLFB."); #endif @@ -257,7 +253,7 @@ void ClearPathMC::move_at_target_velocity() { // Check to see if motor faulted during move if (check_for_faults()) { -#if __SERIAL_DEBUG__ || __CPMC_DEBUG__ +#if __SERIAL_DEBUG__// || __CPMC_DEBUG__ ConnectorUsb.SendLine("Motion may not have completed as expected. Proceed with caution."); } else { diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index b1a1511..b014f72 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -9,15 +9,11 @@ #define __SERIAL_DEBUG__ 0 #endif -#ifndef __ETHUDP_DEBUG__ -#define __ETHUDP_DEBUG__ 0 -#endif - #include "EthUDP.h" EthUDP::EthUDP(): /* Initialize class variables */ - m_local_ip(169, 254, 97, 177), + m_local_ip(169, 254, 57, 177), m_local_port {8888}, m_remote_ip(169, 254, 57, 209), m_remote_port {8888} @@ -96,7 +92,7 @@ void EthUDP::begin(void) { } else { EthernetMgr.LocalIp(m_local_ip); - //EthernetMgr.GatewayIp(IpAddress(169,254, 93, 234)); + //EthernetMgr.GatewayIp(IpAddress(169,254, 93, 234)); // TODO: add these to the constructors EthernetMgr.NetmaskIp(IpAddress(255, 255, 0, 0)); } @@ -151,7 +147,7 @@ void EthUDP::construct_data_msg(slidersystem::DataInterface* state) { strcat(m_msg_buf, m_data_buf); strcat(m_msg_buf, m_msg_footer); -#if __SERIAL_DEBUG__ || __ETHUDP_DEBUG__ +#if __SERIAL_DEBUG__ // || __ETHUDP_DEBUG__ ConnectorUsb.Send("Constructed msg: "); ConnectorUsb.SendLine(m_msg_buf); #endif From c01653bd850dc6c9343e1673476630e343aeb9ce Mon Sep 17 00:00:00 2001 From: lukestroh Date: Fri, 13 Sep 2024 14:04:20 -0700 Subject: [PATCH 19/21] updates to clearpath --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 102912 -> 111104 bytes include/clearpathmc.h | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index baae79e03dcdd7009c69e0168bc5786c53bc354a..3e6bc4593173ce0aa421e41ff5cbc34cded66e59 100644 GIT binary patch delta 4747 zcmd6q3slrq9>?eRXJ(w?CCmU4BJZ~X!!SHU8fHdAO&+F%9YP*MNh%?bAU+t;OwCH> zn*R-ricNFN9?ZWO-Cm?tZd-!2wU)bPZO3!6^58C=ldj5s?*J~^b35&x{m=QFd;j-$ zAHVy%zx(_B$M%HM_PKIfj=#T5CUXOqhlhtz$OuIA6i*hE14m2_n!yYJke=n52;ASuZc!DA@6$Ao5;0`8(P|yr~z)CO?C_w_yfFLjjq=5`@3G?$c z45%jpR87Nis5F6TAQl7wJ-7~_!>|rz81M!w0B=i283p1&WJhr_g>)2$lD4B*n`R#E z!n@5!eE}!|w}4Wx5Zng1eC(<2D4s;iJBl+hgU3mdN9&VNwt#354pxEHU=3Ic)&X{r z>Xiex7{?h`8TxPI@`bo@zR$=CwEvKiV<5ZM$YV;68JWLxBl9u|e;G%8r6_kcEPh%h$6PaYaUzcKTREpVJb*u?2>x@e`yhgYY}!n zv0u*pi`fF>*icd-SCBO&7b=J!A=gI1UGAuKUK=a0u!u-C@y&B*=0g;dq3T6;K0Kav zYr^?pMGRUFvf{`&%(D5kI2cJ$QbFK*3Mnigj^z9z@^L~mb~~v+#eFesv`zUoD#{?~ zLa0)3tUXNZa7Ipdlwe*^RWKGS8b%!)q+lW^46^RxqyXic- z&~O~!5kHNl8p*2LMfL)VipLLr{YK^~=pUKc|4K`tOLsNDD4T&yx|HfB*ezi!ENSmg z9IvPAc=;ea_Ut(MXDFp<+0Rqtx2T;57eFYia1rGtz&AUL65(8Wtt*e%h64#K!Bj%M zEHC~B`%H_9N2&n8^%cT!)H4g@JT!2BVnZ`+%2vFz7+C9#v(&@`Jimx8YxB8L=MNh; z&yvsL71xPbisU?M@hPMsiznI3EppRXLsR)ya;$iPyxw`;_Da&jgUy=3XFWD?b==TJ z48NYHGh?Z3LTNuw88Y$dRGPx9x?pB&h>LH!=Xaa6R*LL0Ww!e8Xf_&?|Mu4Dzr>tf zIk$a@YT=yDRV@N|A zJgK`=>4-T*YT}8G$5bv+xU!&h{+@V&MU*_QCZ5{9C+8KaUAfV3Y?v$QA+675hXVA{ z-4XYZV~bJc8G{dT9)yt$RIJkOCso)ziImbZwus2a$gmE{j|}&*ueXOvUv8fui0xHW zAQkjF6ygI@DS)ZOGu=Chg`Y69oI^Tx_RI%T{%iYHw2#rwHnA;+{Mm%gVzRS^olmk; z55=*)yZo3jKE3xU3!u+dh~|wF5#*?^oUem7D3}ehEdc+ zQzpwS%4dBhqtsVqBxkWF^%3j)+Gy{!+08~)r3+yvd#jkyY?N$H8-^d4#`c;0Snh#b z*4`Dx9y2M~-rhjAlu`G01-Jep~sa0Ou#)j&$ z8BL388tQ7A=2q5K-ci%Q&V4a$B2G>w$c9=ziaN2T$#}Jb@zUc5cL-v!H?3d~erb5| zDJpT3QqElzym*=zdu&qnP@lq=C$+JM-*~m;I}oFA=J`=LiO;LZQ%uy-`z&QR$@Qkt zppTN-PVE*Sk(0_fMNUjE^?tlgz9)p-rGLJuAogy5s_(?HIZ^Hg?vm}4&CNNeq<2UG z!^%9IudAp?!7iNlEav}M_@4^P|IB_q+G~xv{7~`_8vI9O(!|5(+*~`Z<459pg8EjW zo*?SAG&MUqxN|8}%`@WvvnlcB`6KR;UUj=6wOXf3Ov%#MBpUP?S&3OzYf55uN>-(z zT31<>m6^ImESOApimeukadu@>75&;@K;KE4Y}>+m(Gg8SuCIM(ZY~|Xj?$eJN5_=Z zDHfMfvp7&qK5SE(2Fj#5`;*95NnIjMhveg%AUSeF03!zE1+)3Heb~X=U<^!lF1m>p5IuH{wA~N_sovQ$m!5Hh zDp!v+)dxaq5QlH3Rc!yfAm?B)ttHxcrCSVpIm=z_X{IbO^mdx;oWGj(`9qNuDDr@7 zU##V>0O!R@8X_8P$ZX_0Nx~nXMphANWrKVCXu4R`PV1eUtmGhZu#MzmQwbTgwRPXQ zqvEk;q!m{^NH>bjYv?QIvODM;iFYlbsp74Tq;<2_HHy@LF5B9u!ue_~^^o}BCdyIG zs?W1p8)_QGfo0_9!~h-9O>B8z0w3qC!BR`_w$_v_ZLX~^LrAE-ePpFYEm$dNJ(R0m zMg?x-(T0(gy#22lQCpwBs1oy!3_8A%2E^n>9K+swDUqu(m0tnR@i9kyu7rZQNbcOf zoSMh&LEN~RhMZMR^r%2f#7`S3P7Jw>c`-mMv;*vewo$xmJ0y{C;v7tK11C|3R*0 aT$}N`MzwL8!5}%#2U=kvm+dPeP5%O*ipaVE delta 3136 zcmc(heN>cH8o=M@&IdEhs578|*1|9%OvpE)C`hj|Y#S)dm{L-WkRR(NCI`?Zbak4w zQbUEg9d3sVVI;KLtzB;Gq`T$_ny#)1?dq|%HY!i2<0%$hT9bm==MICN&hDOS_mA#5 zzjMEzd++o0zRoLZXNS5mcS@8bNeX1t@AqRR0W#ya_S=YBez?^C#ouT(aX}3KoFu?D zGtQ+TIwS;{fEW=gg4_Litc9E@SSKQrkvk9#G6}gK`4KV)NkgV0VxJo8+mPurH7ODs*@y!XD_3A$j#MBIA*&EEz8Y)#baI?_12(FV zjZOA&*w$oEP&%>aCY9>JjG}*jtyp!;m}13dQN|Ul!qq~-VoWGlD0(9DZ3?~}*W@7M z3RYrU$oOvx#;G!YP{aSTg6HBPLc!u7p=t=UVnvBMi9{-%Wrga zPU834z(%+M%1NTLbxIN+%F}~8*Qyb@Qsg_af_#Ct%`H~m9b80*a-Hf2 zu=7?jt)G9lHjq$7e2c|8@g!`E!rJ4Zh!_536Hyfm)$|5TcDc|3u%pQy0~i94#=W?# z5fOHCBdwU!()VMd4QWR_$N{7Sc^>IR4kCw;7Z7pWu_k-b9j{~OIO0Q2Ai{fkkdw$M zExno; z`_bu--;E!5bg^rlW_eM|WkxXJZj^p1ZNZ$s1id+jwjYV%zN<2LmMGmFJzB|zLheBV zM-C+hqid#3vjhj11}AipAW7meSl9h=6(lk)~2UwiHtYn%^N%(MW|Ny=n!m)7yRPc%T~YcloS zTTV@KoCsK6D>nekNB{?SJmdkl$4XCW_t-nnP{rX`X80F7$oVfx0#kLK}G@Uo=$vpLe+z z$p*jwo9dvke@*-MBdX3tuy;DZ$j+5Q3tQiY3SGGsioRR?0@^Cz?Z8K4-vEusU(e>Z_LyKSKX;6NT zu!dQ8cD*czrY_RtF)T`{<#7Y zusn5?PBWezcKxpISLg0;X|6eR@u=a7uoT@_dL`&EG9xjN$MX(%tEimg{0_MWFGbUD z)bOa`mw-q6J`6z@{MJ)EXQz2yBf8h{mrH!FL!{F*#`g@(?Mb6=A8lv58$m+{Z(3-y z?=SST;Y+mR#s!?{qfx^bC!skcBBeig-QD4;mBq#sR-u6XeE+8vKx0olEX44(?s649 ze<5>%$OmIZ`#)8?t9@Tv zeRA~b@+;=82f_}_lQJ)E@7{{rBwAgwr!yKtA;k;#L7&=Ny#gkxz5OzD6Ys}s!J%f? z*F&B+FAut>HWz};`}76~Bh2~;)Umc2c*pzdVklO7Tih@JG>#akyhjPMn9T*h^zLYe zE?}GY!d>3?pMm#?*RT(ag!vzXrR;nwyvzRVg0t+%R@lt;?tuB;o$b&}n6m?V*`X&P znKf<$rFV8G{2Z9=d058EpM`Ai+(S^HIOy07zy`O0!@K;~@DE^B`yki*$tzF|om(K{ EU!kh&%>V!Z diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 19f0851..3212cee 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -28,7 +28,7 @@ class ClearPathMC { MotorDriver& motor = ConnectorM0; // A reference to the maximum clockwise and counter-clockwise velocities set in - // the MSP software. These must match the values in MSP software + // the MSP software. These must match the values in MSP software. DO NOT CHANGE UNLESS THIS IS ALSO CHANGED. const int32_t m_max_velocity = 1000; const int8_t m_calibration_velocity = -100; From 742ed277b62c1e7a977f63ace45789aca46b7a96 Mon Sep 17 00:00:00 2001 From: lukestroh Date: Fri, 13 Sep 2024 19:06:50 -0700 Subject: [PATCH 20/21] some more edits. Will need a final check and removal of debug prints --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 111104 -> 111104 bytes include/clearpathmc.h | 2 +- main.cpp | 24 ++++++++++----------- src/ClearPathMC.cpp | 6 +++--- src/EthUDP.cpp | 10 ++++----- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 3e6bc4593173ce0aa421e41ff5cbc34cded66e59..62a404aa5c6a8a7a404f0692d736464960e9d162 100644 GIT binary patch delta 487 zcmX}nyGjE=6b9g&akJTos3hJ~xF%jU8rDs`5Ku5`XOu)cMHUHG5(F_s1kqq^;W9-! zktx!Mh0Y`R5`rKI)>bway8ffl;$zO7In4Z1d(~^N`lUn6OUJ0?Zf%f-9OwAP)}Q1d zyCv%3Lq;#pY9{6Rrk3a)4g>--;J~Nv`(Kdco)$|R=wzTD(DbLUCZHXvV1nvro2$qVa0Drk=QUB*ct?ucILW<7Ql2ZuVs*cR}@1nHsX;Y;}k|CYB6u zBO)XFZzNV3uUT;!x1LE9+p!fpOt?(!YW2(*xza5O65C9a*}cn{WbPM<6*X~QZB%Lg E8-E#xeE=QccCi@bN>Me3p*AT(hUo13+Wd9-VEX-pPX~Q_ug~9_vWvR{FSkp6nh&X|6Q19 z|5KC&PmjH#aq(BJuwCN!2od8w6cw}Fts4yl0=?iZxW(2T=n+0|>Wo0?0u4Z4N+R2! z8svc1`jBIw6}Wz;LsgZ}Q}A>;Rhnn)64i)TW}6t}=2|ljMZqel1wVjpqT@Ac>tU@e zX{{ak$r9=}fP*;5f&^#*&yBiRYop!Z(WrhNj2b1UekMw$p9#|-Dv>BZD^}R1vimNn z_`+!vPNbQqaY5u?#zoee5QnT&{Ie!Pl!7|^v=g2bM`g*%-w@6LqOh<;f7DL$bIq3J z2*-A~2UYy6OQ>x7I!Vuz%S?_j+0W!;b@4l6ip}4V$fxJj+&4+`3zr^~DJGYgKV>Ov z motor0.state_.vel) { motor0.set_velocity(motor0.state_.vel + 1); @@ -183,9 +185,8 @@ int main(void) { motor0.calibrate(); // blocking, runs until negative limit switch hit. TODO: change to either side } break; - // check the limit switch statuses + // check the limit switch statuses TODO: does this even need to be checked? Does host ever command a switch? NO. case slidersystem::NEG_LIM: - //curr_vel = -1 * motor0.state_.vel; // negative sign is flipped if (motor0.state_.vel <= 0){ #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); @@ -194,7 +195,6 @@ int main(void) { } break; case slidersystem::POS_LIM: - //curr_vel = -1 * motor0.state_.vel; // negative sign is flipped if (motor0.state_.vel >= 0){ #if __SERIAL_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 2d206bc..74bb77c 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -150,14 +150,14 @@ void ClearPathMC::set_velocity(int vel) { } // check the limit switch statuses - if (vel <= 0 && state_.system_status==slidersystem::NEG_LIM){ + if (vel <= 0 && this->state_.system_status==slidersystem::NEG_LIM){ #if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the negative limit switch"); #endif target_velocity = 0; return; } - if (vel >= 0 && state_.system_status==slidersystem::POS_LIM){ + if (vel >= 0 && this->state_.system_status==slidersystem::POS_LIM){ #if __SERIAL_DEBUG__ || __CPMC_DEBUG__ ConnectorUsb.SendLine("Commanded velocity was stopped by the positive limit switch"); #endif @@ -165,7 +165,7 @@ void ClearPathMC::set_velocity(int vel) { return; } - // Correct command to speed limit + // Correct command to speed direction if (vel > m_max_velocity) { target_velocity = -1 * m_max_velocity; } diff --git a/src/EthUDP.cpp b/src/EthUDP.cpp index b014f72..2646c0f 100644 --- a/src/EthUDP.cpp +++ b/src/EthUDP.cpp @@ -16,7 +16,7 @@ EthUDP::EthUDP(): m_local_ip(169, 254, 57, 177), m_local_port {8888}, m_remote_ip(169, 254, 57, 209), - m_remote_port {8888} + m_remote_port {44644} { } @@ -25,7 +25,7 @@ EthUDP::EthUDP(IpAddress _local_ip): m_local_ip(_local_ip), m_local_port {8888}, m_remote_ip(169, 254, 57, 209), - m_remote_port {8888} + m_remote_port {44644} { @@ -35,7 +35,7 @@ EthUDP::EthUDP(IpAddress _local_ip, int _local_port): m_local_ip(_local_ip), m_local_port(_local_port), m_remote_ip(169, 254, 57, 209), - m_remote_port {8888} + m_remote_port {44644} { } @@ -44,7 +44,7 @@ EthUDP::EthUDP(IpAddress _local_ip, IpAddress _remote_ip): m_local_ip(_local_ip), m_local_port {8888}, m_remote_ip(_remote_ip), - m_remote_port {8888} + m_remote_port {44644} { } @@ -140,7 +140,7 @@ void EthUDP::construct_data_msg(slidersystem::DataInterface* state) { sprintf(m_data_buf, "%f", state->vel); // Create c-str msg - strcat(m_msg_buf, m_msg_status_header); // TODO: For some reason status_header gets set to 0. Needs a debugger. + strcat(m_msg_buf, m_msg_status_header); strcat(m_msg_buf, m_status_buf); strcat(m_msg_buf, m_delimiter); strcat(m_msg_buf, m_msg_data_header); From 193c76ed79c9ede9aabe33e4ffdcb9f1f384e06a Mon Sep 17 00:00:00 2001 From: lukestroh Date: Thu, 26 Sep 2024 12:40:32 -0700 Subject: [PATCH 21/21] added delay to ensure regular message publishing timing --- .vs/ClearCoreVelocityController/v14/.atsuo | Bin 111104 -> 111616 bytes include/clearpathmc.h | 2 +- main.cpp | 13 +++++++++++-- src/ClearPathMC.cpp | 2 +- 4 files changed, 13 insertions(+), 4 deletions(-) diff --git a/.vs/ClearCoreVelocityController/v14/.atsuo b/.vs/ClearCoreVelocityController/v14/.atsuo index 62a404aa5c6a8a7a404f0692d736464960e9d162..dc9d03ede927d337fba6b911aa9789481a972d1a 100644 GIT binary patch delta 3360 zcmchZdr*|u6~Om=`(S}xWO=Iy%Nqm~5Rv%6PgoL#D6B?;FGPb*#z#cN#NsF$ODl1F zuzM1(qlUmI;6`Jjd?{-(8n=p-PU|C`rnS>fLQ<;MRz(Nf!7AHxmtB)a$4>f3@67Kz zckjLDeCM8X&;8cYr?K47G))Z+P$(2iV#wukk#P4DPMAjvYRld~UMa%$!jB;y?+Yc` zFq%y!&=Qe`o0M(!lj#R(x%^3vAoPTR$R|b-IwFHeCAgvwkwZigTyLRAZXl^4B8ec+ zSRToEPd}FAM1s4RLh=#G=?X=?+lZM4(uimxl*l6dh-@N;m_`H>a|!;^ktD|waYUeJ zjE_&D{v^-ubdrh049{==Vw0y|Nb-5&1qF^u4~TiuGd!E*XiuM;<83_s4!4@4(-u)@ z98uv}u#{vav5Z(wR1sWZ1xb86Jw9|Db*hOP&jfcM#A+U|ve1A{vsN|kOHXQ2o{}1$ zgY`yQ$1U>4m7kR&zQ&s(wMUPqXbizq#8b#qq$0d2ilDwXMf_ZRFGcy@94RKy2dV^n)HbE5RCS$ALXlu1fKIOL^@SHMJ z^Bj5mNQ-s4S$Iy!gnGQMtA{w;sGq0eCxpr;S!X0ozAN5res*hB$Pdz2yY+Ffy#!~5 zeV~pfYiH3ss~An;r#eH70vdDdDt_Yl%Wxp4#20w%_^BosyovfLB|gb$8#ii%0^?GQp?lMGsKO^193XRR(ixBW`B9V;qYHoCl;@N@r#|! zUn#8SVEIyM7Ia3<1e`e`4Qr!Iv0IrD{648__{~7A{4%dn1v5{9(M+5IQ7D=$w!&o8 zvxq>Qe~y2^fwl7+R>$a0`<$PIfrb6HpaLFVtPLvhRfwdTPYmymM%?==@Q&055AUy; z2Cfp+(DlYfsKSkjC0g!5I3J>2z$`sufh0fd?5Y87qq*ady-gj$(0S>*KiEM9a8gq^er4T zq+qNu7Q2HpN$aF9EXVxfpxznp^s%L8LS?7p;M_Nhm^PR>2{$;Eb! zg)Bb4V0hA8E@`gHR^wD3EuZ*``#fK*Nd~E0Wy|YUjiALmrt(0Z4x|c|O`?bk8CWZo zYK=4(%jew%BBgR^PV`?a_REpAg$8a>P%=#T8u4O_NS-Y-<@u3`fD z>P(v1FAr|_5o`@Ta?ie8=p5SvD*l<~3hHpW&pwj}C*6Utr`Hn>wQoa9SOFGfWy%9# zIlwBPhg=M(jK}7hSZs-ikexe3kQxQ)h>q=E1YxLUH>J}c?GtQ0*I*ki*nb#%-$_RM z!4NthGiL1K=K@#0J3lbP{lVi;{^28!GMgvJ;$JMncS~2^RHg-QnM#ttf)XH?>8j}( zE4w)sEw@G4c~b-?L_;3tb>*XYM`YiufkeF4wa|7~M2o}1qy!lI6vyqNUOEY~c-I2x z{3K33bZ-gxcO(t}jnODBV6<}|PCb`_fB3YO?P>xo_75iGtp|&xQv$BPQioB_-(uT8 zFJr`mFL1}h0C=nc8oBlCQZ-(@Ji*{}Z;n6Woxi7NB-8rwF6m7_8jask)%>f9I9Nc{ZPcoeU^?wCFvoGNVS z{nlZ*_;)o*j6tJp?CS+~Hwy;Qc}q8mA5s24E}l>l8lv!{8h!Q7{@WOmv&i>fUzv71 z3`zL#?=^=LBKNOB?OahIC|JN`ScM(etC{gfV3HROtOTx&=k8DYHaBwF%_GmgE1oD( z{^em+t07O$yM5LGISz;dm$E|z0Ufa*bcBFkR|Qyn9K9lB18b;(GFDj(qgek6$Yvq= zV0T#kDI|{E6zB=s`zl;xeXC%|(PDxMEnnUSSJ{aq7?1q}cDA(!G^~3Abg_?T!*OU~ zttObyuGB)QqiPP=l}wze9KlZUgLQcM;y$`wm30xoaVZRcaHn&4CdvE|=cl6i8p9Mxw70cTPNqi=NmA8?xJMY0aEUyVl*zFY1vXTQ} zV0#*2n&YijxS(YA?U4RhZy}$V%9bC6NLG1(3gO5z=9J&5~wX zv&0VD9CbUOPjGBK3R{4cwo_ad9D|jtsh<4HXaGGHKMZG|meE@`?}5GS_#Oyim3wGg jkIk~ECTJX1@VY&Mz5Nlq<`8T!6wLyyaL@gq`y=w-hUZNP delta 3288 zcmd6p3s98T7035{yUS}AL_ncN-35{Ga>WM<^4h>gF*H1)Qj#LYXQ~xzC{{uhw`k%c z#>yVzHHsPCB5E|T@)^ITA<2@O2}xs6qb4)XSf{m`rc+B(XiBVX|96FiG&OB9lbPO` z-`;cI_nz0c&JX;YSN(Qmju@fS>GYuA>-D1Y$uY-E?ox~?DUTkp@gvFxO5|RpC?FW4 z@qnFC$y}>%Sw9Aik-!Axayel*m;?NQ85Dyg5CtMY5V#8zfHfc#JPL*ZKadE*z$g$4 zQb7v1h5h9j36LiMh%>$NHZ>p_+zkT2WbiG33EnBF$AJ*=V_*fcKW(6vvZb*kiN-dT zSSY-)Bs?{5$k1HK^FS&14k!Z;g84u$9W09)OCsr!#**ooqlb)49FQYX*MjjN8vF#T z1?xaP_$iP(x%{dO8RH8w?x=D9W}n6fPrZ}ojo9zsH80nEndS{x^Pe<74gEtjU+BB2 zG~=B#FAtiEajAJJ57oT1iqyQch#dP&^Ksv-=509eOEn*Ih+Bdp)b^lF#J^T1`K4n) za3I&2XB$a6DA$ijrX$>IUQe;SI(Uw+J&t>W)vvzJG>&dNs@&I`$8n+0m0L zd!147+>=~%dt^cp=VebaK7)bx)xPk%6}E<*S9>;2BhfvdV%5T^Hlu5#Rkg;)lR-v} zZ+T5Zaexdd>p(}~TM+!@uzJh#hMrGN$#lmmu3ASx6#ArP$W5We+-oc3 zyjAmg-^6j8w@>L(eczimXUhXHmsuleDjN%%VaQOIQ`^D;qAN2Xggoueb6n4jxve$ys#;rPwt6;lk)eQ=lSUt>Q>|G zl%C_>JLtl3#GQI1t|BDv`*D^`7`|j-#Hp{Lfuef%lj2$oZ8>B4ls<)PoW%jf7G+o7I%EK}i}ZAR6&^`=QEMA@9(n#*?0 zE`DdHIKI~U5IZ%y>imln=}!B-nB$}|rgIZ?x^V37uRG1uu}j#0Mj>4Ak9eNdw_n^y zpcKCJ_fq#xg(rRdBOc?qz<>VaBGC++;SC|&;PwmE$GW`q|`6o?&bR6g9efY|y0QI+B#|*<`1}Err zo~d$l8fm&r6rID~jklAQMcZ5Lq0S!05q;Pa2K73cNkm}j%!JDs_@|C81u9z`u z_>J9tOW%twU$MXa#kvNMb(u^q+&tsz&^#i-ER-h9t7xoR`r)&ry&6f_rSF(L7;fr=1maPOG+%O|Sm-bQ)=C3Dg<(U%wb-p=Q%GQ^=mG zc^iKaZGPGR_33;je=^fd8>d8sijS%&Te}=X7xh}%!?c~WYnfCYsGXWcOG!Jknm$p) zTi>Q#no>x$Bv#HLm$s{zx(T2E8Rf;*KYX})L6vppvK33D*;7HOh~7x?Vm$u#X!GXM z5o#@?*psD1in!8D;iC9SnywYBrF%()HdCdzd6X)&Rn2sq#Ex~8BkzrM lysdOqIe7C3ktjYu^Rx$_qrZ{BX_?vw$H+>W;-Y@bzW}N{(?tLP diff --git a/include/clearpathmc.h b/include/clearpathmc.h index 3c74cf2..3212cee 100644 --- a/include/clearpathmc.h +++ b/include/clearpathmc.h @@ -15,7 +15,7 @@ #define CLEARPATHMC_H_ #ifndef __CPMC_DEBUG__ -#define __CPMC_DEBUG__ 1 +#define __CPMC_DEBUG__ 0 #endif // To enable automatic fault handling, #define HANDLE_MOTOR_FAULTS (1) diff --git a/main.cpp b/main.cpp index 11809ee..738b039 100644 --- a/main.cpp +++ b/main.cpp @@ -52,7 +52,7 @@ */ #ifndef __SERIAL_DEBUG__ -#define __SERIAL_DEBUG__ 1 +#define __SERIAL_DEBUG__ 0 #endif #include "ClearCore.h" @@ -135,6 +135,8 @@ int main(void) { e_stop_flag = true; } eth.send_packet(&motor0.state_); + + uint32_t last_time_us = Microseconds(); // Main loop while (true) { @@ -251,7 +253,14 @@ int main(void) { // Move to target velocity (blocking) motor0.move_at_target_velocity(); - // Send status, velocity data to the ROS2 node. + // Send status, velocity data to the ROS2 node. Ensure regular timing (tune if necessary). + while (Microseconds() - last_time_us < 625) { +#if __SERIAL_DEBUG__ + ConnectorUsb.SendLine("Loop wait"); +#endif + } // 571 = int( (1/1750) * 1,0000,000 ) // 1600 is an even 625... + last_time_us = Microseconds(); eth.send_packet(&motor0.state_); + } } \ No newline at end of file diff --git a/src/ClearPathMC.cpp b/src/ClearPathMC.cpp index 74bb77c..29bc03d 100644 --- a/src/ClearPathMC.cpp +++ b/src/ClearPathMC.cpp @@ -192,7 +192,7 @@ void ClearPathMC::set_standby() { void ClearPathMC::move_at_target_velocity() { - /* Move the motor at the set target velocity */ + /* Move the motor at the set target velocity. Update the motor's state. */ double current_motor_velocity = -1 * state_.vel; // Check motor status