Implemented Constant Acceleration Move

firmware/stepper_nano_zero/board.h

@@ -19,10 +19,10 @@
 
 
 //uncomment the follow lines if using the NEMA 23 10A hardware
-#define NEMA_23_10A_HW
+//#define NEMA_23_10A_HW
 
 //uncomment the following if the board uses the A5995 driver (NEMA 23 3.2A boards)
-//#define A5995_DRIVER
+#define A5995_DRIVER
 
 //The March 21 2017 NEMA 17 Smart Stepper has changed some pin outs
 // A1 was changed to read motor voltage, hence SW4 is now using D4
@@ -42,7 +42,7 @@
 
 #define NZS_AS5047_PIPELINE //does a pipeline read of encoder, which is slightly faster
 
-#define NZS_CONTROL_LOOP_HZ (6000) //update rate of control loop
+#define NZS_CONTROL_LOOP_HZ (4000) //update rate of control loop
 
 
 #define NZS_LCD_ABSOULTE_ANGLE  //define this to show angle from zero in positive and negative direction
@@ -54,14 +54,14 @@
 #define VERSION "FW: 0.34" //this is what prints on LCD during splash screen
 
 //Define this to allow command out serial port, else hardware serial is debug log
-//#define CMD_SERIAL_PORT
+// #define CMD_SERIAL_PORT
 
 #define SERIAL_BAUD (115200) //baud rate for the serial ports
 
 //This section is for using the step and dir pins as serial port
 // when the enable pin is inactive.
-#define USE_STEP_DIR_SERIAL
-#define STEP_DIR_BAUD (19200) //this is the baud rate we will use
+// #define USE_STEP_DIR_SERIAL
+// #define STEP_DIR_BAUD (19200) //this is the baud rate we will use
 
 // These are used as an attempt to use TC4 to count steps
 //  currently this is not working.
@@ -504,3 +504,4 @@ static inline void DelayMs(uint32_t ms)
 
 #endif//__BOARD_H__
 
+

firmware/stepper_nano_zero/commands.cpp

@@ -4,7 +4,7 @@
 #include "stepper_controller.h"
 #include <stdlib.h>
 #include "nonvolatile.h"
-#include "reset.h"
+#include "Reset.h"
 #include "nzs.h"
 #include "ftoa.h"
 #include "board.h"
@@ -62,6 +62,7 @@ CMD_STR(stepsperrotation, "gets/set the motor steps per rotation, should only be
 //CMD_STR(motorparams, "with no arguments read parameters, will set with arguments");
 CMD_STR(boot, "Enters the bootloader");
 CMD_STR(move, "moves encoder to absolute angle in degrees 'move 400.1'");
+CMD_STR(moveCA, "constant Accel move to a absolute angles 'move pos maxrpm'");
 //CMD_STR(printdata, "prints last n error terms");
 CMD_STR(velocity, "gets/set velocity in RPMs");
 CMD_STR(factoryreset, "resets board to factory defaults");
@@ -119,6 +120,7 @@ sCommand Cmds[] =
 		//COMMAND(motorparams),
 		COMMAND(boot),
 		COMMAND(move),
+		COMMAND(moveCA),
 		//COMMAND(printdata),
 		COMMAND(velocity),
 		COMMAND(factoryreset),
@@ -1054,6 +1056,28 @@ static int move_cmd(sCmdUart *ptrUart,int argc, char * argv[])
 	return 0;
 }
 
+static int moveCA_cmd(sCmdUart *ptrUart,int argc, char * argv[])
+{
+	int32_t x,ma;
+	//CommandPrintf(ptrUart, "Move %d",argc);
+
+	if (2 == argc)
+	{
+		float f,maxrpm,a,y;
+		float pos,dx;
+
+		f=atof(argv[0]);
+		maxrpm=atof(argv[1]);
+		//		if (f>1.8)
+		//			f=1.8;
+		//		if (f<-1.8)
+		//			f=-1.8;
+
+		SmartPlanner.moveConstantAccel(f,maxrpm);
+	}
+
+	return 0;
+}
 static int boot_cmd(sCmdUart *ptrUart,int argc, char * argv[])
 {
 	initiateReset(250);
@@ -1532,3 +1556,4 @@ int commandsProcess(void)
 #endif
 	return CommandProcess(&UsbUart,Cmds,' ',COMMANDS_PROMPT);
 }
+

firmware/stepper_nano_zero/nzs.cpp

@@ -664,7 +664,6 @@ void NZS::begin(void)
 #else
 	Lcd.lcdShow("MisfitTech","NEMA 17", VERSION);
 #endif
-
 #endif
 
 	LOG("command init!");

firmware/stepper_nano_zero/nzs_lcd.cpp

@@ -586,3 +586,4 @@ void NZS_LCD::updateLCD(void)
 }
 
 
+

firmware/stepper_nano_zero/planner.cpp

@@ -130,6 +130,40 @@ void Planner::tick(void)
 		}
 	}
 
+	if (PLANNER_CA == currentMode)
+	{
+//		SerialUSB.println(currentSetAngle);
+//		SerialUSB.println(endAngle);
+//		SerialUSB.println(tickIncrement);
+//		SerialUSB.println(fabs(currentSetAngle-endAngle));
+//		SerialUSB.println(fabs(tickIncrement*2));
+//		SerialUSB.println();
+		int32_t x;
+                float t;
+		if (fabs(currentSetAngle-endAngle) >= 0.5)
+		{
+                        t = fabs((currentSetAngle-endAngle) / (startAngle-endAngle));
+		        if (t < 0.5)
+                        {
+                        currentVelocity=maxVelocity*t*2;
+                        }
+                        else
+                        {
+                        currentVelocity=2*maxVelocity-maxVelocity*t*2;
+                        }
+			currentSetAngle+=currentVelocity;
+			x=ANGLE_FROM_DEGREES(currentSetAngle);
+			ptrStepperCtrl->moveToAbsAngle(x);
+		}
+                else
+		{
+			//we are done, make sure we end at the right point
+			//SerialUSB.println("done");
+			x=ANGLE_FROM_DEGREES(endAngle);
+			ptrStepperCtrl->moveToAbsAngle(x);
+			currentMode=PLANNER_NONE;
+		}
+	}
 
 }
 
@@ -186,3 +220,48 @@ bool Planner::moveConstantVelocity(float finalAngle, float rpm)
 	exitTC3CriticalSection(state);
 	return true;
 }
+
+
+bool Planner::moveConstantAccel(float finalAngle, float maxrpm)
+{
+	bool state;
+	state = enterTC3CriticalSection();
+
+	//first determine if operation is in progress
+	if (PLANNER_NONE != currentMode)
+	{
+		//we are in operation return false
+		SerialUSB.println("planner operational");
+		exitTC3CriticalSection(state);
+		return false;
+	}
+
+	//get current posistion
+	startAngle = ANGLE_T0_DEGREES(ptrStepperCtrl->getCurrentAngle());
+
+	//deterime the max velocity
+        maxVelocity=369.0*fabs(maxrpm)/60/PLANNER_UPDATE_RATE_HZ;
+
+	//set the desired end angle
+	endAngle=finalAngle;
+
+	//set the current angle
+        currentSetAngle=startAngle+4;
+
+	if (startAngle>endAngle)
+	{
+		SerialUSB.println("reverse");
+		maxVelocity=-maxVelocity;
+	        currentSetAngle=startAngle-4;
+	}
+
+//	SerialUSB.println(currentSetAngle);
+//		SerialUSB.println(endAngle);
+//		SerialUSB.println(tickIncrement);
+//		SerialUSB.println();
+
+	currentMode=PLANNER_CA;
+
+	exitTC3CriticalSection(state);
+	return true;
+}

firmware/stepper_nano_zero/planner.h

@@ -27,7 +27,7 @@
 typedef enum {
 	PLANNER_NONE =0,
 	PLANNER_CV =1, //constant velocity
-	//PLANNER_CA =2, //constant accleration
+	PLANNER_CA =2, //constant accleration
 	//PLANNER_S_CURVE =3, //s-curve move
 } PlannerMode;
 class Planner
@@ -40,10 +40,13 @@ class Planner
 		volatile float startAngle;
 		volatile float currentSetAngle;
 		volatile float tickIncrement;
+		volatile float currentVelocity;
+                volatile float maxVelocity;
 
 	public:
 		void begin(StepperCtrl *ptrStepper);
 		bool moveConstantVelocity(float finalAngle, float rpm); //moves to the final location at a constant RPM
+		bool moveConstantAccel(float finalAngle, float maxrpm); //moves to the final location at a constant RPM
 		void tick(void); //this is called on regulat tick interval
 		void stop(void);
 		bool done(void) {return currentMode==PLANNER_NONE;}