// 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);

	// Set the target velocity
	od_write(0x60FF, 0x00, 50);

 	// 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(1000);

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

	// Let the motor run for a while
	sleep(1000);
	
	// 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);
}
