Motion Profile Driver

firmware/RAD/release/.cproject

@@ -38,6 +38,7 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/PID_lib/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/queue/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/AT24C04C/Inc}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/Motion_Profiler/Drivers/Motion_Profiler}&quot;"/>
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.461382603" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
 							</tool>
@@ -60,6 +61,7 @@
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/PID_lib/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/queue/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/AT24C04C/Inc}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;C:\Users\ishan\OneDrive\Documents\MarsRover\maxwell\firmware\STM32 Drivers\Motion_Profiler\Drivers\Motion_Profiler\Inc&quot;"/>
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c.195043681" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.compiler.input.c"/>
 							</tool>
@@ -69,6 +71,7 @@
 							</tool>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.1130552671" name="MCU GCC Linker" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script.1973020258" name="Linker Script (-T)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.script" value="${workspace_loc:/${ProjName}/STM32F103C8TX_FLASH.ld}" valueType="string"/>
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="true" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries.1811713496" name="Libraries (-l)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.option.libraries" valueType="libs"/>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input.387490637" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.c.linker.input">
 									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
 									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
@@ -85,10 +88,6 @@
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec.664266173" name="MCU Output Converter Motorola S-rec with symbols" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.objcopy.symbolsrec"/>
 						</toolChain>
 					</folderInfo>
-					<sourceEntries>
-						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
-						<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
-					</sourceEntries>
 				</configuration>
 			</storageModule>
 			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
@@ -119,12 +118,13 @@
 							<builder buildPath="${workspace_loc:/RAD}/Release" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.2144270885" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1227910564" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">
 								<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.1595922698" name="Debug level" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel" value="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.debuglevel.value.g0" valueType="enumerated"/>
-								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.1144161618" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
+								<option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths.1144161618" name="Include paths (-I)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.option.includepaths" valueType="includePath">
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/TMC_2590/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/AS5048A/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/PID_lib/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/queue/Inc}&quot;"/>
 									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/${ProjName}/Drivers/AT24C04C/Inc}&quot;"/>
+									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/Motion_Profiler/Drivers/Motion_Profiler}&quot;"/>
 								</option>
 								<inputType id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input.222322327" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.input"/>
 							</tool>

firmware/RAD/release/.project

@@ -55,5 +55,10 @@
 			<type>2</type>
 			<locationURI>PARENT-2-PROJECT_LOC/STM32%20Drivers/queue/Drivers/queue</locationURI>
 		</link>
+		<link>
+			<name>Drivers/Motion_Profiler</name>
+			<type>2</type>
+			<locationURI>PARENT-2-PROJECT_LOC/STM32%20Drivers/Motion_Profiler/Drivers/Motion_Profiler</locationURI>
+		</link>
 	</linkedResources>
 </projectDescription>

firmware/RAD/release/Core/Inc/motionprofile.h

@@ -0,0 +1,17 @@
+/*
+ * motionprofile.h
+ *
+ *  Created on: Dec 13, 2024
+ *      Author: Ishan
+ */
+
+#ifndef INC_MOTIONPROFILE_H_
+#define INC_MOTIONPROFILE_H_
+
+#include <motion_profiler.h>
+
+extern Motion_Profiler_HandleTypeDef motion_profile_1;
+
+void MX_Motion_Profile_1_Init(void);
+
+#endif /* INC_MOTIONPROFILE_H_ */

firmware/RAD/release/Core/Src/main.c

@@ -33,6 +33,7 @@
 #include "pid.h"
 #include "queue.h"
 #include "enc_dec_utils.h"
+#include "motionprofile.h"
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -223,6 +224,7 @@ int main(void)
     MX_TMC_2590_1_Init();
     MX_AS5048A_1_Init();
     MX_PID_1_Init();
+    MX_Motion_Profile_1_Init();
 
 
     switch(rad_params.RAD_TYPE)
@@ -336,6 +338,8 @@ int main(void)
                         new_setpoint = max_angle;
                     }
                     PID_ChangeSetPoint(&pid_1, new_setpoint*MOTOR_GEARING);
+
+                    Motion_Profiler_GenerateNewTrajectory(&motion_profile_1, (double) (pid_1.feedback_adj / MOTOR_GEARING), new_setpoint);
                     break;
                 }
                 case GET_ENCODER_VALUE:
@@ -1034,6 +1038,23 @@ int main(void)
 
                     //Don't update PID unless we have a new value
                     PID_Update_BangBang(&pid_1);
+
+                    Motion_Profiler_GetVelocity(&motion_profile_1, (double) (pid_1.feedback_adj / MOTOR_GEARING));
+
+                    //find autoreloadregister value associated with new velocity
+                    uint32_t arr = HAL_TIM_CalculateAutoReload(tmc_2590_1.Init.STEP_Tim,
+                       motion_profile_1.current_velocity);
+
+                    //2 assign ARR to timer
+
+                    TMC_2590_SetTimAutoReload(&tmc_2590_1, arr);
+
+                    //3 update local stepper speed reference to the integer value
+
+                    rad_params.STEPPER_SPEED = HAL_TIM_CalculateFrequency(tmc_2590_1.Init.STEP_Tim);
+
+
+
                 }
 
 
@@ -1099,6 +1120,10 @@ int main(void)
                 {
                     rad_status.TMC_STATUS = TMC_2590_MoveSteps(&tmc_2590_1, (int16_t) pid_1.output);
                 }
+                else
+                {
+                    TMC_2590_Stop(&tmc_2590_1);
+                }
 
                 break;
             }

firmware/RAD/release/Core/Src/motionprofile.c

@@ -0,0 +1,25 @@
+/*
+ * motionprofile.c
+ *
+ *  Created on: Dec 13, 2024
+ *      Author: Ishan
+ */
+
+#include "motionprofile.h"
+
+Motion_Profiler_HandleTypeDef motion_profile_1;
+
+void MX_Motion_Profile_1_Init(void)
+{
+    motion_profile_1.Init.max_velocity = 2000;
+    motion_profile_1.Init.min_velocity = 500;
+
+    motion_profile_1.Init.acceleration = 5000; //1 second to ramp up/down
+    motion_profile_1.Init.deadzone = 10;
+
+
+    Motion_Profiler_Init(&motion_profile_1);
+
+
+}
+

firmware/RAD/release/can-testbench/writer.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+
+# follow the link to install python-can. Be sure to pip install python-can[pcan] if using pcan
+import can  # https://python-can.readthedocs.io/en/stable/installation.html
+import struct
+import time
+
+
+def main():
+    sleep_time = 3
+    with can.ThreadSafeBus(interface="pcan", channel="PCAN_USBBUS1", bitrate=500_000) as bus:
+
+        #send_can_id(bus=bus, id=0x1A54, value=0xB0)
+
+        while True:
+            i = input("command? ")
+
+            if (i == 'exit'):
+                break
+            elif(i == "set pid"):
+                send_p_value(bus=bus, id=0x14, value=0.04)
+            elif(i == "get pid"):
+                get_p_value(bus=bus, id=0x14, value=0.04)
+            elif(i == "step motor"):
+                step_motor(bus=bus, id=0x14, value=50)
+                #     get_p_value(bus=bus, id=0x14, value=0.04)
+
+                #     step_motor(bus=bus, id=0x14, value=-50)
+                #     continue
+
+
+
+
+
+def send_setpoint(bus: can.BusABC, id: int, value: float):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x04),
+        data=struct.pack(">f", value),
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+
+def send_reset(bus: can.BusABC, id: int):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x49),
+        data=[],
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+
+def send_p_value(bus: can.BusABC, id: int, value: float):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x05),
+        data=struct.pack(">f", value),
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+def get_p_value(bus: can.BusABC, id: int, value: float):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x06),
+        data=struct.pack(">f", value),
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+def step_motor(bus: can.BusABC, id: int, value: float):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x51),
+        data=struct.pack(">f", value),
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+
+def send_i_value(bus: can.BusABC, id: int, value: float):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x47),
+        data=struct.pack(">f", value),
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+
+def send_d_value(bus: can.BusABC, id: int, value: float):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0x48),
+        data=struct.pack(">f", value),
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+
+def send_can_id(bus: can.BusABC, id: int, value: int):
+    new_msg = can.Message(
+        arbitration_id=__can_message_id(id, 0),
+        data=[0, 0, 0, 0, 0, 0, 0, value],
+        is_extended_id=True,
+    )
+    bus.send(msg=new_msg)
+
+
+def __can_message_id(device_id, command_id):
+    print((2 << 25) | (command_id << 8) | device_id)
+    return (2 << 25) | (command_id << 8) | device_id
+
+
+if __name__ == "__main__":
+    main()