Last active
November 24, 2019 16:30
-
-
Save oak-tree/ad244ae2075d0a664055b3f5f07b148a to your computer and use it in GitHub Desktop.
motor
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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)); | |
} |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// 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); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment