motor

motor.cpp

// in this example the position mode will be selected and started, after the input 1 is triggered, the motor moves to a set position (flagposition mode)

//1. Step: mapping the frequently used SDO´s
map U16 ControlWord as output 0x6040:00
map U32 ProfileVelocity as output 0x6081:00
map S32 TargetPosition as output 0x607A:00
map U32 Inputs as input 0x60FD:00
map S32 ActualPosition as input 0x6064:00
map S32 AnalogInput as input 0x3320:01
map S08 HomingMethod as output 0x6098:00


static bool forward; 

#include "wrapper.h"

#define HOMING_MODE 6
#define PROFILE_POSITION 1
#define DELTA 1000
#define TARGET_POSITION 3075400
#define RPM 1000
//2. Step: call Main function and set the speed and mode of operation
void user()
{

	forward = true;
	od_write(0x6060,0x00, HOMING_MODE);								// set the mode of operation to profile position
	Out.HomingMethod = 35;
	
	/*
	od_write(0x6060,0x00, PROFILE_POSITION);								// set the mode of operation to profile position
	Out.ProfileVelocity = 500;							//sets the profile velocity to 200 rpm
	Out.TargetPosition = -370000;					// setting the target position (just as a limit)
	*/
		
//3. Step: switch on the state machine
	Out.ControlWord = 0x6;				// switch to the "enable voltage" state
	do 	{
		yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x21);    // wait until drive is in state "enable voltage"	
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0001


	
	Out.ControlWord = 0x7;	// switch to the "switched on" state
	do 	{
			yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x23);   // wait until drive is in state "switched on" 
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0011		
		
	Out.ControlWord = 0x4F;	// switch to the "enable operation" state , target position relative
	do 	{
			yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x27);   // wait until drive is in state "operation enabled"	
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0111	
	
	Out.ControlWord = 0x5F;								// start	
	yield();
		
		
	
	
	od_write(0x6060,0x00, PROFILE_POSITION);								// set the mode of operation to profile position
	Out.ProfileVelocity = RPM;	 	//sets the profile velocity to 200 rpm
	if (forward) {
			Out.TargetPosition = TARGET_POSITION;
	}
	else {
			Out.TargetPosition = -1 * TARGET_POSITION;					// setting the target position (just as a limit)
	}
		
//3. Step: switch on the state machine
	Out.ControlWord = 0x6;				// switch to the "enable voltage" state
	do 	{
		yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x21);    // wait until drive is in state "enable voltage"	
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0001


	
	Out.ControlWord = 0x7;	// switch to the "switched on" state
	do 	{
			yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x23);   // wait until drive is in state "switched on" 
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0011		
		
	Out.ControlWord = 0x4F;	// switch to the "enable operation" state , target position relative
	do 	{
			yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x27);   // wait until drive is in state "operation enabled"	
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0111	
	
	Out.ControlWord = 0x5F;								// start	
	yield();
	do {
		yield();
	}
	while ((forward && In.ActualPosition < TARGET_POSITION) ||  (!forward && In.ActualPosition > 0 ));
	
	forward = false;
		
		
	od_write(0x6060,0x00, PROFILE_POSITION);								// set the mode of operation to profile position
	Out.ProfileVelocity = RPM;	 	//sets the profile velocity to 200 rpm
	if (forward) {
			Out.TargetPosition = TARGET_POSITION;
	}
	else {
			Out.TargetPosition = -1 * TARGET_POSITION;					// setting the target position (just as a limit)
	}
		
//3. Step: switch on the state machine
	Out.ControlWord = 0x6;				// switch to the "enable voltage" state
	do 	{
		yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x21);    // wait until drive is in state "enable voltage"	
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0001


	
	Out.ControlWord = 0x7;	// switch to the "switched on" state
	do 	{
			yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x23);   // wait until drive is in state "switched on" 
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0011		
		
	Out.ControlWord = 0x4F;	// switch to the "enable operation" state , target position relative
	do 	{
			yield();						// waiting for the next cycle (1ms)
		}
		while ( (od_read(0x6041, 0x00) & 0xEF) != 0x27);   // wait until drive is in state "operation enabled"	
		// checking the statusword (0x6041) for the bitmask: xxxx xxxx x01x 0111	
	
	Out.ControlWord = 0x5F;								// start	
	yield();
	do {
		yield();
	}
	while ((forward && In.ActualPosition < TARGET_POSITION) ||  (!forward && In.ActualPosition > 0  + DELTA));
	
	
}	

motor2.cpp

// In this NanoJ program, we will make the motor turn shortly back and forth.
// The language used for NanoJ programs is C, with a few specific extensions,
// like the mappings (see below).
// Please refer to the product manual for more details about NanoJ and the
// object dictionary.

// You can map frequently used objects to be able to read or write them
// using In.* and Out.*. Here we map the object 6041:00 as "In.StatusWord".
map U16 StatusWord as input 0x6041:00

// Include the definition of NanoJ functions and symbols
#include "wrapper.h"

// The user() function is the entry point of the NanoJ program. It is called
// by the firmware of the controller when the NanoJ program is started.
void user()
{
	// Set mode "Profile velocity"
	od_write(0x6060, 0x00, 3);

	// Remember target velocity before overwriting, so we can reset it later.
	U32 targetVelocity = od_read(0x60FF, 0x00);

	//od_write(0x6064, 0x00, 0x00);
	
	od_write(0x2061, 0x00, 1);
	od_write(0x2062, 0x00, 60);
	
	// Set the target velocity
	od_write(0x60FF, 0x00, 10);

 	// Request state "Ready to switch on"
	od_write(0x6040, 0x00, 0x6);

	// Wait until the requested state is reached
	while ( (In.StatusWord & 0xEF) != 0x21) {
		yield(); // Wait for the next cycle (1ms)
	}

	// Request the state "Switched on"
	od_write(0x6040, 0x00, 0x7);

	// Wait until the requested state is reached
	while ( (In.StatusWord & 0xEF) != 0x23) {
		yield();
	}

	// Request the state "Operation enabled"
	od_write(0x6040, 0x00, 0xF);

	// Wait until the requested state is reached
	while ( (In.StatusWord & 0xEF) != 0x27) {
		yield();
	}

	// Let the motor run for a while
	sleep(2000);

	// Set the target velocity to run in the opposite direction
	od_write(0x60FF, 0x00, -10);

	// Let the motor run for a while
	sleep(2000);

	
	// Stop the motor
	od_write(0x6040, 0x00, 0x0);

	// Reset the target velocity to its previous value
	od_write(0x60FF, 0x00, targetVelocity);

	// Stop the NanoJ program. Without this line, the firmware would
	// call user() again as soon as we return.
	od_write(0x2300, 0x00, 0x0);
}