Close

Development: Controlling the stepper motors

A project log for Building the Thor robot

Building a 6-axis robot based on the Thor robot. When size DOES matter!

Olaf BaeyensOlaf Baeyens 05/28/2017 at 21:170 Comments

It took hard work to map and test every single control bit but I give to the community the compressed functionality to control the 7 stepper motors simultaneously used on the UltraTronics board v1.0 which is in fact an Arduino Due on steroids.

(Constants see below)

The code below has had real functional testing with real motors.

void MotorCommand::SendMotorStepCommand(byte step_command) {
    // Make the signal high
    SendMotorStepHighCommand(step_command);
    StepMotorPulseDelay2Micros();

    // Make the signal low
    SendMotorStepLowCommand(step_command);
    StepMotorPulseDelay2Micros();
}

void MotorCommand::StepMotorPulseDelay2Micros() {
	// Code blow should be about 2 µS on a 84 Mhz Arduino Due
	for (auto i = 0; i < 36; ++i) {
		asm("nop \n");
	}
}

void MotorCommand::SetPinModes(byte enable_command) {
	pinMode(Z_AXIS_DIRECTION, OUTPUT);
	pinMode(Z_AXIS_STEP, OUTPUT);
	pinMode(Z_AXIS_ENABLE, OUTPUT);

	pinMode(Y_AXIS_DIRECTION, OUTPUT);
	pinMode(Y_AXIS_STEP, OUTPUT);
	pinMode(Y_AXIS_ENABLE, OUTPUT);

	pinMode(X_AXIS_DIRECTION, OUTPUT);
	pinMode(X_AXIS_STEP, OUTPUT);
	pinMode(X_AXIS_ENABLE, OUTPUT);

	pinMode(E3_AXIS_DIRECTION, OUTPUT);
	pinMode(E3_AXIS_STEP, OUTPUT);
	pinMode(E3_AXIS_ENABLE, OUTPUT);

	pinMode(E2_AXIS_DIRECTION, OUTPUT);
	pinMode(E2_AXIS_STEP, OUTPUT);
	pinMode(E2_AXIS_ENABLE, OUTPUT);

	pinMode(E1_AXIS_DIRECTION, OUTPUT);
	pinMode(E1_AXIS_STEP, OUTPUT);
	pinMode(E1_AXIS_ENABLE, OUTPUT);

	pinMode(E0_AXIS_DIRECTION, OUTPUT);
	pinMode(E0_AXIS_STEP, OUTPUT);
	pinMode(E0_AXIS_ENABLE, OUTPUT);
}

void MotorCommand::SendMotorEnableCommand(byte enable_command)  {
	byte level = enable_command;

	level = enable_command & MOTOR_BIT_Z;
	if (level != 0) REG_PIOA_SODR = PIO_PA15;		// Z_AXIS_ENABLE set to high, bit 7
	else REG_PIOA_CODR = PIO_PA15;					// Z_AXIS_ENABLE set to low, bit 7

	level = enable_command & MOTOR_BIT_Y;
	if (level != 0) REG_PIOC_SODR = PIO_PC1;		// Y_AXIS_ENABLE set to high, bit 6
	else REG_PIOC_CODR = PIO_PC1;					// Y_AXIS_ENABLE set to low, bit 6

	level = enable_command & MOTOR_BIT_X; 
	if (level != 0) REG_PIOC_SODR = PIO_PC5;		// X_AXIS_ENABLE set to high, bit 5
	else REG_PIOC_CODR = PIO_PC5;					// X_AXIS_ENABLE set to low, bit 5

	level = enable_command & MOTOR_BIT_E3;
	if (level != 0) REG_PIOC_SODR = PIO_PC8;		// E3_AXIS_ENABLE set to high; bit 4
	else REG_PIOC_CODR = PIO_PC8;					// E6_AXIS_ENABLE set to low, bit 4
	
	level = enable_command & MOTOR_BIT_E2;  
	if (level != 0) REG_PIOA_SODR = PIO_PA20;		// E2_AXIS_ENABLE set to high, bit 3
	else REG_PIOA_CODR = PIO_PA20;					// E2_AXIS_ENABLE set to low, bit 3

	level = enable_command & MOTOR_BIT_E1; 
	if (level != 0) REG_PIOC_SODR = PIO_PC18;		// E1_AXIS_ENABLE set to high, bit 2
	else REG_PIOC_CODR = PIO_PC18;					// E1_AXIS_ENABLE set to low, bit 2

	level = enable_command & MOTOR_BIT_E0;			
	if (level != 0) REG_PIOC_SODR = PIO_PC15;		// E0_AXIS_ENABLE set to high, bit 1
	else REG_PIOC_CODR = PIO_PC15;					// E0_AXIS_ENABLE set to low, bit 1

	StepMotorPulseDelay2Micros();
}

void MotorCommand::SendMotorDirectionCommand(byte direction_command) {
	byte level = direction_command;

	level = direction_command & MOTOR_BIT_Z;
	if (level != 0) REG_PIOD_SODR = PIO_PD1;		// Z_AXIS_DIRECTION set to high, bit 7
	else REG_PIOD_CODR = PIO_PD1;					// Z_AXIS_DIRECTION set to low, bit 7

	level = direction_command & MOTOR_BIT_Y; 
	if (level != 0) REG_PIOA_SODR = PIO_PA14;		// Y_AXIS_DIRECTION set to high, bit 6
	else REG_PIOA_CODR = PIO_PA14;					// Y_AXIS_DIRECTION set to low, bit 6

	level = direction_command & MOTOR_BIT_X; 
	if (level != 0) REG_PIOC_SODR = PIO_PC2;		// X_AXIS_DIRECTION set to high, bit 5
	else REG_PIOC_CODR = PIO_PC2;					// X_AXIS_DIRECTION set to low, bit 5

	level = direction_command & MOTOR_BIT_E3; 
	if (level != 0) REG_PIOC_SODR = PIO_PC6;		// E3_AXIS_DIRECTION set to high; bit 4
	else REG_PIOC_CODR = PIO_PC6;					// E3_AXIS_DIRECTION set to low, bit 4

	level = direction_command & MOTOR_BIT_E2; 
	if (level != 0) REG_PIOC_SODR = PIO_PC9;		// E2_AXIS_DIRECTION set to high, bit 3
	else REG_PIOC_CODR = PIO_PC9;					// E2_AXIS_DIRECTION set to low, bit 3

	level = direction_command & MOTOR_BIT_E1;
	if (level != 0) REG_PIOC_SODR = PIO_PC4;		// E1_AXIS_DIRECTION set to high, bit 2
	else REG_PIOC_CODR = PIO_PC4;					// E1_AXIS_DIRECTION set to low, bit 2

	level = direction_command & MOTOR_BIT_E0;
	if (level != 0) REG_PIOC_SODR = PIO_PC17;		// E0_AXIS_DIRECTION set to high, bit 1
	else REG_PIOC_CODR = PIO_PC17;					// E0_AXIS_DIRECTION set to low, bit 1

	StepMotorPulseDelay2Micros();
}

void MotorCommand::SendMotorStepHighCommand(byte step_command) {
	byte level = step_command;

	level = step_command & MOTOR_BIT_Z;
	if (level != 0) REG_PIOD_SODR = PIO_PD0;		// Z_AXIS_STEP set to high, bit 7

	level = step_command & MOTOR_BIT_Y;
	if (level != 0) REG_PIOB_SODR = PIO_PB26;		// Y_AXIS_STEP set to high, bit 6

	level = step_command & MOTOR_BIT_X;
	if (level != 0) REG_PIOC_SODR = PIO_PC3;		// X_AXIS_STEP set to high, bit 5

	level = step_command & MOTOR_BIT_E3;
	if (level != 0) REG_PIOC_SODR = PIO_PC7;		// E3_AXIS_STEP set to high; bit 4

	level = step_command & MOTOR_BIT_E2;
	if (level != 0) REG_PIOA_SODR = PIO_PA19;		// E2_AXIS_STEP set to high, bit 3

	level = step_command & MOTOR_BIT_E1;
	if (level != 0) REG_PIOC_SODR = PIO_PC19;		// E1_AXIS_STEP set to high, bit 2

	level = step_command & MOTOR_BIT_E0;
	if (level != 0) REG_PIOC_SODR = PIO_PC16;		// E0_AXIS_STEP set to high, bit 1
}

void MotorCommand::SendMotorStepLowCommand(byte step_command) {
	byte level = step_command;

	level = step_command & MOTOR_BIT_Z;
	if (level != 0) REG_PIOD_CODR = PIO_PD0;		// Z_AXIS_STEP set to low, bit 7

	level = step_command & MOTOR_BIT_Y;
	if (level != 0) REG_PIOB_CODR = PIO_PB26;		// Y_AXIS_STEP set to Low, bit 6 	

	level = step_command & MOTOR_BIT_X;
	if (level != 0) REG_PIOC_CODR = PIO_PC3;		// X_AXIS_STEP set to Low, bit 5

	level = step_command & MOTOR_BIT_E3;
	if (level != 0)REG_PIOC_CODR = PIO_PC7;		    // E3_AXIS_STEP set to low, bit 4

	level = step_command & MOTOR_BIT_E2;
	if (level != 0) REG_PIOA_CODR = PIO_PA19;		// E2_AXIS_STEP set to low, bit 3

	level = step_command & MOTOR_BIT_E1;
	if (level != 0) REG_PIOC_CODR = PIO_PC19;		// E0_AXIS_STEP set to low, bit 2

	level = step_command & MOTOR_BIT_E0;
	if (level != 0)REG_PIOC_CODR = PIO_PC16;		// E1_AXIS_STEP set to low, bit 1
}


Ultratronics v1.0 constants used

#define __SAM3X8E__

#define  X_AXIS_DIRECTION 34			// D34 X axis direction --> PC2 
#define  X_AXIS_STEP 35					// D35 X axis step  	--> PC3
#define  X_AXIS_ENABLE 37				// D37 X axis enable	--> PC5
//BasicStepperDriver stepperX(MOTOR_STEPS, 34, 35, 37); // X-axis

#define  Y_AXIS_DIRECTION 23			// D23 Y axis direct	--> PA14
#define  Y_AXIS_STEP 22					// D22 Y axis step		--> PB26
#define  Y_AXIS_ENABLE 33				// D33 Y axis enable	--> PC1
//BasicStepperDriver stepperY(MOTOR_STEPS, 23, 22, 33); // Z-axis

#define  Z_AXIS_DIRECTION 26			//D26 Z axis direction	--> PD1
#define  Z_AXIS_STEP 25					//D25 Z axis step		--> PD0
#define  Z_AXIS_ENABLE 24				//D24 Z axis enable		--> PA15  
//BasicStepperDriver stepperZ(MOTOR_STEPS, 26, 25, 24); // Z-axis

#define  E0_AXIS_DIRECTION 46			//D46 E0 axis direction	--> PC17
#define  E0_AXIS_STEP 47				//D47 E0 axis step		--> PC16
#define  E0_AXIS_ENABLE 48				//D48 E0 axis enable	--> PC15
//BasicStepperDriver stepperE0(MOTOR_STEPS, 46, 47, 48); // Z-axis

#define  E1_AXIS_DIRECTION 36			//D36 E1 axis direction	--> PC4
#define  E1_AXIS_STEP 44				//D44 E1 axis step		--> PC19
#define  E1_AXIS_ENABLE 45				//D45 E1 axis enable	--> PC18
//BasicStepperDriver stepperE1(MOTOR_STEPS, 36, 44, 45); // Z-axis

#define  E2_AXIS_DIRECTION 41			//D41 E2 axis direction	--> PC9
#define  E2_AXIS_STEP 42				//D42 E2 axis step		--> PA19
#define  E2_AXIS_ENABLE 43				//D43 E2 axis enable	--> PA20
//BasicStepperDriver stepperE2(MOTOR_STEPS, 41, 42, 43); // Z-axis

#define  E3_AXIS_DIRECTION 38			//D38 E3 axis direction	--> PC6
#define  E3_AXIS_STEP 39				//D39 E3 axis step		--> PC7
#define  E3_AXIS_ENABLE 40				//D40 E3 axis enable	--> PC8
//BasicStepperDriver stepperE3(MOTOR_STEPS, 38, 39, 40); // Z-axis
//BasicStepperDriver stepper(MOTOR_STEPS, DIR, STEP, ENBL);

And the bits to sets (but you can define it yourself)

#define MOTOR_BIT_Z  0b10000000
#define MOTOR_BIT_Y  0b01000000
#define MOTOR_BIT_X  0b00100000
#define MOTOR_BIT_E3 0b00010000
#define MOTOR_BIT_E2 0b00001000
#define MOTOR_BIT_E1 0b00000100
#define MOTOR_BIT_E0 0b00000010

It is used something like:

	byte StepCommand = MOTOR_BIT_Z | MOTOR_BIT_X;
	MotorCommand::SendMotorStepCommand(StepCommand);

Discussions