| ApossC SDK V01.15

SDK_Amplifier_DS402_StateMachine.mc
Go to the documentation of this file.
1
14 #pragma once
15
16 #include <SysDef.mh>
17 #include "SDK_Amplifier_DS402_StateMachine.mh"
18
19
20
33 long sdkDS402_ReadStatusWord(long busId)
34 {
35 return SdoRead(busId, DS402_STATUSWORD, 0);
36 }
37
46 long sdkDS402_ReadControlWord(long busId)
47 {
48 return SdoRead(busId, DS402_CONTROLWORD, 0);
49 }
50
60 void sdkDS402_WriteControlWord(long busId, long value)
61 {
62 SdoWrite(busId, DS402_CONTROLWORD, 0, value);
63 }
64
65
83 long sdkDS402_GetActDriveState(long busId)
84 {
85 long status = sdkDS402_ReadStatusWord(busId);
86
87 switch(status & 0x006F) //mask bits xxxx xxxx x11x 1111
88 {
89
90 case 0x0000: return(DS402_DRIVE_STATE_NOT_READY_TO_SWITCH_ON); //xxxx xxxx x00x 0000
91 case 0x0040: return(DS402_DRIVE_STATE_SWITCH_ON_DISABLED); //xxxx xxxx x10x 0000
92 case 0x0021: return(DS402_DRIVE_STATE_READY_TO_SWITCH_ON); //xxxx xxxx x01x 0001
93 case 0x0023: return(DS402_DRIVE_STATE_SWITCHED_ON); //xxxx xxxx x01x 0011
94 case 0x0027: return(DS402_DRIVE_STATE_OPERATION_ENABLED); //xxxx xxxx x01x 0111
95 case 0x0007: return(DS402_DRIVE_STATE_QUICK_STOP_ACTIVE); //xxxx xxxx x01x 0001
96 case 0x000F: return(DS402_DRIVE_STATE_FAULT_REACTION_ACTIVE); //xxxx xxxx x00x 1111
97 case 0x0008: return(DS402_DRIVE_STATE_FAULT); //xxxx xxxx x00x 1000
98 default: return(-1);;
99 }
100 }
101
102
111 void sdkDS402_PrintState(long state)
112 {
113 switch(state)
114 {
115 case DS402_DRIVE_STATE_NOT_READY_TO_SWITCH_ON: printf("DRIVE_STATE_NOT_READY_TO_SWITCH_ON"); break;
116 case DS402_DRIVE_STATE_SWITCH_ON_DISABLED: printf("DRIVE_STATE_SWITCH_ON_DISABLED"); break;
117 case DS402_DRIVE_STATE_READY_TO_SWITCH_ON: printf("DRIVE_STATE_READY_TO_SWITCH_ON"); break;
118 case DS402_DRIVE_STATE_SWITCHED_ON: printf("DRIVE_STATE_SWITCHED_ON"); break;
119 case DS402_DRIVE_STATE_OPERATION_ENABLED: printf("DRIVE_STATE_OPERATION_ENABLED"); break;
120 case DS402_DRIVE_STATE_QUICK_STOP_ACTIVE: printf("DRIVE_STATE_QUICK_STOP_ACTIVE"); break;
121 case DS402_DRIVE_STATE_FAULT_REACTION_ACTIVE: printf("DRIVE_STATE_FAULT_REACTION_ACTIVE"); break;
122 case DS402_DRIVE_STATE_FAULT: printf("DRIVE_STATE_FAULT"); break;
123 default: printf("%d", state);
124 }
125 }
126
152 long helperDS402_ExecuteTransition(long busId, long transition)
153 {
154 long controlWord = sdkDS402_ReadControlWord(busId);
155 long retVal = 1;
156
157 switch(transition) {
158 case DS402_TRANSITION_SHUTDOWN:
159 //Transition Shutdown: CW 0xxx x110
160 printf("...: execute Transition: DS402_TRANSITION_SHUTDOWN\n", busId);
161 controlWord = (controlWord & 0xFF78) | 0x06;
162 sdkDS402_WriteControlWord(busId, controlWord);
163 break;
164 case DS402_TRANSITION_SWITCH_ON:
165 printf("...: execute Transition: DS402_TRANSITION_SWITCH_ON\n", busId);
166 //Transition Switch On: CW 0xxx x111
167 controlWord = (controlWord & 0xFF78) | 0x07;
168 sdkDS402_WriteControlWord(busId, controlWord);
169 break;
170 case DS402_TRANSITION_ENABLE_OPERATION:
171 printf("...: execute Transition: DS402_TRANSITION_ENABLE_OPERATION\n", busId);
172 //Transition Enable Operation: CW 0xxx 1111
173 controlWord = (controlWord & 0xFF70) | 0x0F;
174 sdkDS402_WriteControlWord(busId, controlWord);
175 break;
176 case DS402_TRANSITION_DISABLE_VOLTAGE:
177 printf("...: execute Transition: DS402_TRANSITION_DISABLE_VOLTAGE\n", busId);
178 //Transition Disable Voltage: CW 0xxx xx0x
179 controlWord = (controlWord & 0xFF7D);
180 sdkDS402_WriteControlWord(busId, controlWord);
181 break;
182 case DS402_TRANSITION_QUICK_STOP:
183 printf("...: execute Transition: DS402_TRANSITION_QUICK_STOP\n", busId);
184 //Transition Quick Stop: CW 0xxx x01x
185 controlWord = (controlWord & 0xFF79) | 0x02;
186 sdkDS402_WriteControlWord(busId, controlWord);
187 break;
188 case DS402_TRANSITION_DISABLE_OPERATION:
189 printf("...: execute Transition: DS402_TRANSITION_DISABLE_OPERATION\n", busId);
190 //Transition Disable Voltage: CW 0xxx 0111
191 controlWord = (controlWord & 0xFF70) | 0x07;
192 sdkDS402_WriteControlWord(busId, controlWord);
193 break;
194 case DS402_TRANSITION_FAULT_RESET:
195 printf("...: execute Transition: DS402_TRANSITION_FAULT_RESET\n", busId);
196 //Transition Fault Resest: CW 0xxx xxxx -> 1xxx xxxx
197 controlWord = (controlWord & 0xFF7F);
198 sdkDS402_WriteControlWord(busId, controlWord);
199 //Delay(1);
200 controlWord = controlWord | 0x80;
201 sdkDS402_WriteControlWord(busId, controlWord);
202 //Transition switch on disable
203 controlWord = controlWord & 0xFF7F;
204 sdkDS402_WriteControlWord(busId, controlWord);
205 break;
206 default:
207 printf("Invalid state transition\n");
208 retVal = -1;
209 break;
210 }
211
212 return(retVal);
213 }
214
215
230 long sdkDS402_TransitionToState(long busId, long newState) {
231
232 long transition = -1;
233 long actState = sdkDS402_GetActDriveState(busId);
234
235 printf("Bus Id %d, Transition from ", busId);
236 sdkDS402_PrintState(actState);
237 printf(" to ");
238 sdkDS402_PrintState(newState);
239 print();
240
241 switch(actState) {
242 case DS402_DRIVE_STATE_NOT_READY_TO_SWITCH_ON:
243 if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
244 // Automatic transition
245 }
246 break;
247 case DS402_DRIVE_STATE_SWITCH_ON_DISABLED:
248 if (newState == DS402_DRIVE_STATE_READY_TO_SWITCH_ON) {
249 transition = DS402_TRANSITION_SHUTDOWN;
250 }
251 break;
252 case DS402_DRIVE_STATE_READY_TO_SWITCH_ON:
253 if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
254 transition = DS402_TRANSITION_DISABLE_VOLTAGE;
255 }
256 else if (newState == DS402_DRIVE_STATE_SWITCHED_ON) {
257 transition = DS402_TRANSITION_SWITCH_ON;
258 }
259 else if (newState == DS402_DRIVE_STATE_OPERATION_ENABLED) {
260 transition = DS402_TRANSITION_ENABLE_OPERATION;
261 }
262 break;
263 case DS402_DRIVE_STATE_SWITCHED_ON:
264 if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
265 transition = DS402_TRANSITION_DISABLE_VOLTAGE;
266 }
267 else if (newState == DS402_DRIVE_STATE_READY_TO_SWITCH_ON) {
268 transition = DS402_TRANSITION_SHUTDOWN;
269 }
270 else if (newState == DS402_DRIVE_STATE_OPERATION_ENABLED) {
271 transition = DS402_TRANSITION_ENABLE_OPERATION;
272 }
273 break;
274 case DS402_DRIVE_STATE_OPERATION_ENABLED:
275 if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
276 transition = DS402_TRANSITION_DISABLE_VOLTAGE;
277 }
278 else if (newState == DS402_DRIVE_STATE_READY_TO_SWITCH_ON) {
279 transition = DS402_TRANSITION_SHUTDOWN;
280 }
281 else if (newState == DS402_DRIVE_STATE_SWITCHED_ON) {
282 transition = DS402_TRANSITION_DISABLE_OPERATION;
283 }
284 else if (newState == DS402_DRIVE_STATE_QUICK_STOP_ACTIVE) {
285 transition = DS402_TRANSITION_QUICK_STOP;
286 }
287 break;
288 case DS402_DRIVE_STATE_QUICK_STOP_ACTIVE:
289 if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
290 transition = DS402_TRANSITION_SHUTDOWN;
291 }
292 else if (newState == DS402_DRIVE_STATE_OPERATION_ENABLED) {
293 transition = DS402_TRANSITION_ENABLE_OPERATION;
294 }
295 break;
296 case DS402_DRIVE_STATE_FAULT_REACTION_ACTIVE:
297 if (newState == DS402_DRIVE_STATE_FAULT) {
298 // Automatic transition
299 }
300 else if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
301 transition = DS402_TRANSITION_FAULT_RESET;
302 }
303 break;
304 case DS402_DRIVE_STATE_FAULT:
305 if (newState == DS402_DRIVE_STATE_SWITCH_ON_DISABLED) {
306 transition = DS402_TRANSITION_FAULT_RESET;
307 }
308 break;
309 default:
310 printf("...: invalid actual drive state: %d\n", busId, actState);
311 break;
312 }
313
314 if(transition != -1)
315 {
316 helperDS402_ExecuteTransition(busId, transition);
317 }
318 else
319 {
320 printf("...: invalid state transition\n", busId);
321 }
322
323 return(transition);
324 }
325
347 long sdkDS402_WaitTransitionToState(long busId, long targetState, long timeout)
348 {
349 long actState, transition;
350 long time = Time();
351
352 printf("Bus Id %d, sdkDS402_WaitTransitionToState\n" ,busId);
353
354 transition = sdkDS402_TransitionToState(busId, targetState);
355 if(transition < 0)
356 {
357 //Invalid transition
358 return(-1);
359 }
360
361 actState = sdkDS402_GetActDriveState(busId);
362 while(actState != targetState)
363 {
364 if(timeout >= 0 && (Time() - time) > timeout)
365 {
366 //Timeout elapsed
367 printf("...: target state not reached within timeout (%d ms)\n", timeout);
368 return(-2);
369 }
370 actState = sdkDS402_GetActDriveState(busId);
371 }
372
373 return(transition);
374 }
375
376
377
389 void sdkDS402_SetProfileAcceleration(long busId, long acc)
390 {
391 SdoWrite(busId, DS402_PROFILE_ACCELERATION, 0, acc);
392 }
393
401 void sdkDS402_SetProfileDeceleration(long busId, long dec)
402 {
403 SdoWrite(busId, DS402_PROFILE_DECELERATION, 0, dec);
404 }
405
414 void sdkDS402_SetProfileVelocity(long busId, long vel)
415 {
416 SdoWrite(busId, DS402_PROFILE_VELOCITY, 0, vel);
417 }
418
427 void sdkDS402_SetMotionProfileType(long busId, long type)
428 {
429 SdoWrite(busId, DS402_MOTION_PROFILE_TYPE, 0, type);
430 }
431
439 void sdkDS402_SetMaxProfileVelocity(long busId, long vel)
440 {
441 SdoWrite(busId, DS402_MAX_PROFILE_VELOCITY, 0, vel);
442 }
443
452 void sdkDS402_SetMaxMotorSpeed(long busId, long vel)
453 {
454 SdoWrite(busId, DS402_MAX_MOTOR_SPEED, 0, vel);
455 }
456
464 void sdkDS402_SetMaxAcceleration(long busId, long acc)
465 {
466 SdoWrite(busId, DS402_MAX_ACCELERATION, 0, acc);
467 }
468
476 void sdkDS402_SetQuickStopDeceleration(long busId, long dec)
477 {
478 SdoWrite(busId, DS402_QUICK_STOP_DECELERATION, 0, dec);
479 }
480
488 void sdkDS402_SetProfileMovementParameter(long busId, long acc, long dec, long velocity)
489 {
490 sdkDS402_SetProfileAcceleration(busId, acc);
491 sdkDS402_SetProfileDeceleration(busId, dec);
492 sdkDS402_SetProfileVelocity(busId, velocity);
493 }
494
495
508 void sdkDS402_SetOpartionMode(long busId, long operationMode)
509 {
510 if( operationMode == DS402_OP_PPM)
511 {
512 printf("Bus Id %d, setup profile position mode\n" ,busId);
513 SdoWrite( busId, DS402_MODES_OF_OPERATION, 0, DS402_OP_PPM);
514 }
515 else if(operationMode == DS402_OP_PVM)
516 {
517 printf("Bus Id %d, setup profile velocity mode\n" ,busId);
518 SdoWrite( busId, DS402_MODES_OF_OPERATION, 0, DS402_OP_PVM);
519 }
520 }
521
531 long sdkDS402_GetOpartionMode(long busId)
532 {
533 long operationMode = SdoRead( busId, DS402_MODES_OF_OPERATION, 0);
534 return(operationMode);
535 }
536
544 void sdkDS402_QuickStop(long busId)
545 {
546 printf("Bus Id %d, Quick Stop\n", busId);
547 sdkDS402_TransitionToState(busId, DS402_DRIVE_STATE_QUICK_STOP_ACTIVE);
548 }
549
557 void sdkDS402_Halt(long busId)
558 {
559 long controlWord;
560 printf("Bus Id %d, Halt\n", busId);
561
562 controlWord = sdkDS402_ReadControlWord(busId);
563 controlWord.i[DS402_CW_BIT_HALT] = 1;
564
565 sdkDS402_WriteControlWord(busId, controlWord);
566 }
567
574 void sdkDS402_ResetFault(long busId)
575 {
576 printf("Bus Id %d, Reset Fault\n", busId);
577 sdkDS402_TransitionToState(busId, DS402_DRIVE_STATE_SWITCH_ON_DISABLED);
578 }
579
588 void sdkDS402_Print_StatusWord(long busId)
589 {
590 long value;
591 long status;
592 long operationMode;
593 operationMode = SdoRead(busId, DS402_MODES_OF_OPERATION, 0);
594 status = sdkDS402_ReadStatusWord(busId);
595
596
597 print("STATUS WORD");
598
599 value = status.i[DS402_SW_BIT_READY_TO_SWITCH_ON];
600 printf("\tREADY_TO_SWITCH_ON:\t%d\n", value);
601
602 value = status.i[DS402_SW_BIT_SWITCHED_ON];
603 printf("\tSWITCHED_ON:\t\t%d\n", value);
604
605 value = status.i[DS402_SW_BIT_OPERATION_ENABLED];
606 printf("\tOPERATION_ENABLED:\t%d\n", value);
607
608 value = status.i[DS402_SW_BIT_FAULT];
609 printf("\tFAULT:\t\t\t%d\n", value);
610
611 value = status.i[DS402_SW_BIT_VOLTAGE_ENABLED];
612 printf("\tVOLTAGE_ENABLED:\t%d\n", value);
613
614 value = status.i[DS402_SW_BIT_QUICK_STOP];
615 printf("\tQUICK_STOP:\t\t%d\n", value);
616
617 value = status.i[DS402_SW_BIT_SWITCH_ON_DISABLED];
618 printf("\tSWITCH_ON_DISABLED:\t%d\n", value);
619
620 value = status.i[DS402_SW_BIT_WARNING];
621 printf("\tWARNING:\t\t%d\n", value);
622
623 value = status.i[DS402_SW_BIT_REMOTE];
624 printf("\tREMOTE:\t\t\t%d\n", value);
625
626 value = status.i[DS402_SW_BIT_TARGET_REACHED];
627 printf("\tTARGET_REACHED:\t\t%d\n", value);
628
629 value = status.i[DS402_SW_BIT_INTERNAL_LIMIT];
630 printf("\tINTERNAL_LIMIT:\t\t%d\n", value);
631
632 if(operationMode == DS402_OP_PPM)
633 {
634 value = status.i[DS402_SW_PPM_BIT_SETPOINT_ACK];
635 printf("\tSETPOINT_ACK:\t\t%d\n", value);
636
637 value = status.i[DS402_SW_PPM_BIT_FOLLWING_ERROR];
638 printf("\tFOLLWING_ERROR:\t\t%d\n", value);
639 }
640
641 if(operationMode == DS402_OP_PVM)
642 {
643 value = status.i[DS402_SW_PVM_BIT_SPEED];
644 printf("\tSPEED:\t\t\t%d\n", value);
645 }
646 }
647
656 void sdkDS402_Print_ControlWord(long busId)
657 {
658 long value;
659 long controlWord;
660 long operationMode;
661
662 operationMode = SdoRead(busId, DS402_MODES_OF_OPERATION, 0);
663 controlWord = sdkDS402_ReadControlWord(busId);
664
665 print("CONTROL WORD");
666 value = controlWord.i[DS402_CW_BIT_SWITCH_ON];
667 printf("\tSWITCH_ON:\t\t%d\n", value);
668
669 value = controlWord.i[DS402_CW_BIT_ENABLE_VOLTAGE];
670 printf("\tENABLE_VOLTAGE:\t\t%d\n", value);
671
672 value = controlWord.i[DS402_CW_BIT_QUICK_STOP];
673 printf("\tQUICK_STOP:\t\t%d\n", value);
674
675 value = controlWord.i[DS402_CW_BIT_ENABLE_OPERATION];
676 printf("\tENABLE_OPERATION:\t%d\n", value);
677
678 value = controlWord.i[DS402_CW_BIT_HALT];
679 printf("\tHALT:\t\t\t%d\n", value);
680
681 if(operationMode == DS402_OP_PPM)
682 {
683 value = controlWord.i[DS402_CW_PPM_BIT_NEW_SETPOINT];
684 printf("\tNEW_SETPOINT:\t%d\n", value);
685
686 value = controlWord.i[DS402_CW_PPM_BIT_CHANGE_IMMEDIATELY];
687 printf("\tCHANGE_IMMEDIATELY:\t%d\n", value);
688
689 value = controlWord.i[DS402_CW_PPM_BIT_RELATIVE_MOVEMENT];
690 printf("\tRELATIVE_MOVEMENT:\t%d\n", value);
691
692 value = controlWord.i[DS402_CW_PPM_BIT_ENDLESS_MOVEMENT];
693 printf("\tENDLESS_MOVEMENT:\t%d\n", value);
694 }
695 }
696
697
721 long helperDS402_PPM_PosStart(long busId, long pos, long relative, long startImmediately)
722 {
723 long retVal = 1;
724 long controlWord = 0;
725 long setpointAcknowlege=1;
726 long time = 0;
727 long actState = -1;
728
729 printf("...: new setpoint: %d\n", pos);
730
731 if(sdkDS402_ReadStatusWord(busId).i[DS402_SW_PPM_BIT_SETPOINT_ACK]==1)
732 {
733 print("...: the previous setpoint has been assumed and no additional setpoint may be accepted");
734 return(-2);
735 }
736
737 actState = sdkDS402_GetActDriveState(busId);
738 if(actState != DS402_DRIVE_STATE_OPERATION_ENABLED)
739 {
740 print("...: drive state is not in operation enabled");
741 return(-1);
742 }
743
744 SdoWrite(busId, DS402_TARGET_POSITION, 0 ,pos);
745
746 controlWord = sdkDS402_ReadControlWord(busId);
747 controlWord.i[DS402_CW_PPM_BIT_NEW_SETPOINT] = 1;
748 controlWord.i[DS402_CW_PPM_BIT_CHANGE_IMMEDIATELY] = startImmediately;
749 controlWord.i[DS402_CW_PPM_BIT_RELATIVE_MOVEMENT] = relative;
750 controlWord.i[DS402_CW_PPM_BIT_ENDLESS_MOVEMENT] = 0;
751
752 sdkDS402_WriteControlWord(busId, controlWord);
753
754 time = Time();
755 while(sdkDS402_ReadStatusWord(busId).i[DS402_SW_PPM_BIT_SETPOINT_ACK]!=1)
756 {
757 if((Time()-time) > 2) //is blocking until the new setpoint has been acknowledged
758 {
759 print("...: no setpoint acknowledge received");
760 setpointAcknowlege = 0;
761 retVal = -1;
762 break;
763 }
764 }
765 controlWord = sdkDS402_ReadControlWord(busId);
766 controlWord.i[DS402_CW_PPM_BIT_NEW_SETPOINT] = 0;
767 controlWord.i[DS402_CW_PPM_BIT_CHANGE_IMMEDIATELY] = 0;
768 controlWord.i[DS402_CW_PPM_BIT_RELATIVE_MOVEMENT] = 0;
769 controlWord.i[DS402_CW_PPM_BIT_ENDLESS_MOVEMENT] = 0;
770
771 sdkDS402_WriteControlWord(busId, controlWord);
772
773 if(setpointAcknowlege)
774 {
775 print("...: setpoint acknowleged");
776 }
777
778 return(retVal);
779 }
780
781
797 long sdkDS402_PPM_PosAbsStart(long busId, long pos, long startImmediately)
798 {
799
800 printf("Bus Id %d, sdkDS402_PPM_PosAbsStart:\n", busId);
801 return helperDS402_PPM_PosStart(busId, pos, DS402_PPM_ABSOLUTE, startImmediately);
802 }
803
819 long sdkDS402_PPM_PosRelStart(long busId, long pos, long startImmediately)
820 {
821 printf("Bus Id %d, sdkDS402_PPM_PosRelStart:\n", busId);
822 return helperDS402_PPM_PosStart(busId, pos, DS402_PPM_RELATIVE, startImmediately);
823 }
824
825
836 long sdkDS402_PPM_TargetReached(long busId)
837 {
838 return(sdkDS402_ReadStatusWord(busId).i[DS402_SW_BIT_TARGET_REACHED]);
839 }
840
853 long sdkDS402_PPM_WaitTargetReached(long busId, long timeout)
854 {
855 long time = Time();
856 long targetReached = 1;
857
858 printf("Bus Id %d, sdkDS402_PPM_WaitTargetReached:\n", busId);
859
860 while(sdkDS402_PPM_TargetReached(busId) == 0 && targetReached)
861 {
862 if(timeout >= 0 && (Time() - time) > timeout)
863 {
864 targetReached = -1;
865 printf("...: target position not reached within timeout (%d ms)\n", timeout);
866 }
867 }
868 if(targetReached)
869 {
870 print("...: target position reached");
871 }
872
873 return(targetReached);
874 }
875
876
888 void sdkDS402_PVM_SetTargetVelocity(long busId, long targetVelocity)
889 {
890 printf("Bus Id %d, PVM: Set target velocity to %d\n", busId, targetVelocity);
891 SdoWrite(busId, DS402_TARGET_VELOCITY, 0, targetVelocity);
892 }
893
904 long sdkDS402_PVM_CvelStart(long busId)
905 {
906 long actState;
907 long controlWord;
908
909 printf("Bus Id %d, sdkDS402_PPM_CvelStart: \n", busId);
910
911 actState = sdkDS402_GetActDriveState(busId);
912 if(actState != DS402_DRIVE_STATE_OPERATION_ENABLED)
913 {
914 printf("...: drive state is not in operation enabled\n");
915 return(-1);
916 }
917
918 //Reset the Halt bit to start the execution
919 controlWord = sdkDS402_ReadControlWord(busId);
920 controlWord.i[DS402_CW_BIT_HALT] = 0;
921 sdkDS402_WriteControlWord(busId, controlWord);
922
923 printf("...: started\n");
924
925 return(1);
926 }
927
935 long sdkDS402_PVM_CvelStop(long busId)
936 {
937 long controlWord;
938 printf("Bus Id %d, sdkDS402_PPM_CvelStop:\n", busId);
939
940 //Set the Halt bit, to stop the execution
941 controlWord = sdkDS402_ReadControlWord(busId);
942 controlWord.i[DS402_CW_BIT_HALT] = 1;
943
944 sdkDS402_WriteControlWord(busId, controlWord);
945 printf("...: stopped\n");
946
947 return(1);
948 }
949
959 long sdkDS402_PVM_TargetReached(long busId)
960 {
961 return sdkDS402_ReadStatusWord(busId).i[DS402_SW_BIT_TARGET_REACHED];
962 }
963
976 long sdkDS402_PVM_WaitTargetReached(long busId, long timeout)
977 {
978 long time = Time();
979 long targetReached = 1;
980
981 printf("Bus Id %d, sdkDS402_PVM_WaitTargetReached:\n", busId);
982
983 while(sdkDS402_PPM_TargetReached(busId) == 0 && targetReached)
984 {
985 if(timeout >= 0 && (Time() - time) > timeout)
986 {
987 targetReached = -1;
988 printf("...: target velocity not reached within timeout (%d ms)\n", timeout);
989 }
990 }
991 if(targetReached)
992 {
993 print("...: target velocity reached");
994 }
995
996 return(targetReached);
997 }
DS402_SW_BIT_SWITCH_ON_DISABLED
#define DS402_SW_BIT_SWITCH_ON_DISABLED
Definition: SDK_Amplifier_DS402_StateMachine.mh:103
sdkDS402_Print_StatusWord
void sdkDS402_Print_StatusWord(long busId)
Print the status word.
Definition: SDK_Amplifier_DS402_StateMachine.mc:588
sdkDS402_ReadStatusWord
long sdkDS402_ReadStatusWord(long busId)
DS 402 State-Machine functions.
Definition: SDK_Amplifier_DS402_StateMachine.mc:33
sdkDS402_PVM_TargetReached
long sdkDS402_PVM_TargetReached(long busId)
Checks whether the target velocity has reached.
Definition: SDK_Amplifier_DS402_StateMachine.mc:959
sdkDS402_ResetFault
void sdkDS402_ResetFault(long busId)
Reset fault.
Definition: SDK_Amplifier_DS402_StateMachine.mc:574
DS402_SW_BIT_TARGET_REACHED
#define DS402_SW_BIT_TARGET_REACHED
Definition: SDK_Amplifier_DS402_StateMachine.mh:106
DS402_SW_BIT_OPERATION_ENABLED
#define DS402_SW_BIT_OPERATION_ENABLED
Definition: SDK_Amplifier_DS402_StateMachine.mh:99
sdkDS402_PPM_PosRelStart
long sdkDS402_PPM_PosRelStart(long busId, long pos, long startImmediately)
Start relative positioning.
Definition: SDK_Amplifier_DS402_StateMachine.mc:819
DS402_DRIVE_STATE_SWITCH_ON_DISABLED
#define DS402_DRIVE_STATE_SWITCH_ON_DISABLED
Definition: SDK_Amplifier_DS402_StateMachine.mh:61
DS402_STATUSWORD
#define DS402_STATUSWORD
Definition: SDK_Amplifier_DS402_StateMachine.mh:96
DS402_MAX_PROFILE_VELOCITY
#define DS402_MAX_PROFILE_VELOCITY
Definition: SDK_Amplifier_DS402_StateMachine.mh:139
DS402_DRIVE_STATE_FAULT_REACTION_ACTIVE
#define DS402_DRIVE_STATE_FAULT_REACTION_ACTIVE
Definition: SDK_Amplifier_DS402_StateMachine.mh:66
sdkDS402_SetProfileVelocity
void sdkDS402_SetProfileVelocity(long busId, long vel)
Set the profile velocity DS402_PROFILE_VELOCITY(0x6081)
Definition: SDK_Amplifier_DS402_StateMachine.mc:414
DS402_TARGET_VELOCITY
#define DS402_TARGET_VELOCITY
Definition: SDK_Amplifier_DS402_StateMachine.mh:184
DS402_DRIVE_STATE_QUICK_STOP_ACTIVE
#define DS402_DRIVE_STATE_QUICK_STOP_ACTIVE
Definition: SDK_Amplifier_DS402_StateMachine.mh:65
DS402_SW_BIT_QUICK_STOP
#define DS402_SW_BIT_QUICK_STOP
Definition: SDK_Amplifier_DS402_StateMachine.mh:102
sdkDS402_GetActDriveState
long sdkDS402_GetActDriveState(long busId)
Get the actual drive state.
Definition: SDK_Amplifier_DS402_StateMachine.mc:83
DS402_TRANSITION_SWITCH_ON
#define DS402_TRANSITION_SWITCH_ON
Definition: SDK_Amplifier_DS402_StateMachine.mh:71
DS402_TARGET_POSITION
#define DS402_TARGET_POSITION
Definition: SDK_Amplifier_DS402_StateMachine.mh:136
DS402_PROFILE_ACCELERATION
#define DS402_PROFILE_ACCELERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:142
sdkDS402_GetOpartionMode
long sdkDS402_GetOpartionMode(long busId)
Get the mode of operation DS402_MODES_OF_OPERATION(0x6060)
Definition: SDK_Amplifier_DS402_StateMachine.mc:531
DS402_MAX_ACCELERATION
#define DS402_MAX_ACCELERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:175
DS402_CW_BIT_HALT
#define DS402_CW_BIT_HALT
Definition: SDK_Amplifier_DS402_StateMachine.mh:88
DS402_TRANSITION_DISABLE_VOLTAGE
#define DS402_TRANSITION_DISABLE_VOLTAGE
Definition: SDK_Amplifier_DS402_StateMachine.mh:73
DS402_MOTION_PROFILE_TYPE
#define DS402_MOTION_PROFILE_TYPE
Definition: SDK_Amplifier_DS402_StateMachine.mh:145
sdkDS402_TransitionToState
long sdkDS402_TransitionToState(long busId, long newState)
Transition to another state.
Definition: SDK_Amplifier_DS402_StateMachine.mc:230
DS402_SW_BIT_VOLTAGE_ENABLED
#define DS402_SW_BIT_VOLTAGE_ENABLED
Definition: SDK_Amplifier_DS402_StateMachine.mh:101
DS402_TRANSITION_FAULT_RESET
#define DS402_TRANSITION_FAULT_RESET
Definition: SDK_Amplifier_DS402_StateMachine.mh:76
DS402_SW_BIT_REMOTE
#define DS402_SW_BIT_REMOTE
Definition: SDK_Amplifier_DS402_StateMachine.mh:105
DS402_CW_PPM_BIT_RELATIVE_MOVEMENT
#define DS402_CW_PPM_BIT_RELATIVE_MOVEMENT
Definition: SDK_Amplifier_DS402_StateMachine.mh:93
DS402_CW_BIT_QUICK_STOP
#define DS402_CW_BIT_QUICK_STOP
Definition: SDK_Amplifier_DS402_StateMachine.mh:86
sdkDS402_SetProfileMovementParameter
void sdkDS402_SetProfileMovementParameter(long busId, long acc, long dec, long velocity)
Set quick stop deceleration DS402_QUICK_STOP_DECELERATION(0x6085)
Definition: SDK_Amplifier_DS402_StateMachine.mc:488
DS402_SW_BIT_WARNING
#define DS402_SW_BIT_WARNING
Definition: SDK_Amplifier_DS402_StateMachine.mh:104
DS402_MAX_MOTOR_SPEED
#define DS402_MAX_MOTOR_SPEED
Definition: SDK_Amplifier_DS402_StateMachine.mh:140
sdkDS402_SetQuickStopDeceleration
void sdkDS402_SetQuickStopDeceleration(long busId, long dec)
Set quick stop deceleration DS402_QUICK_STOP_DECELERATION(0x6085)
Definition: SDK_Amplifier_DS402_StateMachine.mc:476
DS402_TRANSITION_ENABLE_OPERATION
#define DS402_TRANSITION_ENABLE_OPERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:72
helperDS402_PPM_PosStart
long helperDS402_PPM_PosStart(long busId, long pos, long relative, long startImmediately)
Profile Position Mode functions.
Definition: SDK_Amplifier_DS402_StateMachine.mc:721
sdkDS402_Print_ControlWord
void sdkDS402_Print_ControlWord(long busId)
Print the control word.
Definition: SDK_Amplifier_DS402_StateMachine.mc:656
DS402_PROFILE_VELOCITY
#define DS402_PROFILE_VELOCITY
Definition: SDK_Amplifier_DS402_StateMachine.mh:141
sdkDS402_SetMaxAcceleration
void sdkDS402_SetMaxAcceleration(long busId, long acc)
Set the max acceleration DS402_MAX_ACCELERATION(0x60C5)
Definition: SDK_Amplifier_DS402_StateMachine.mc:464
DS402_DRIVE_STATE_FAULT
#define DS402_DRIVE_STATE_FAULT
Definition: SDK_Amplifier_DS402_StateMachine.mh:67
sdkDS402_PPM_WaitTargetReached
long sdkDS402_PPM_WaitTargetReached(long busId, long timeout)
Wait until target is reachd.
Definition: SDK_Amplifier_DS402_StateMachine.mc:853
sdkDS402_SetMotionProfileType
void sdkDS402_SetMotionProfileType(long busId, long type)
Set the motion profile type DS402_MOTION_PROFILE_TYPE(0x6086)
Definition: SDK_Amplifier_DS402_StateMachine.mc:427
sdkDS402_Halt
void sdkDS402_Halt(long busId)
Stop the motor.
Definition: SDK_Amplifier_DS402_StateMachine.mc:557
DS402_SW_BIT_SWITCHED_ON
#define DS402_SW_BIT_SWITCHED_ON
Definition: SDK_Amplifier_DS402_StateMachine.mh:98
DS402_CW_BIT_ENABLE_OPERATION
#define DS402_CW_BIT_ENABLE_OPERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:87
sdkDS402_ReadControlWord
long sdkDS402_ReadControlWord(long busId)
Read control word.
Definition: SDK_Amplifier_DS402_StateMachine.mc:46
DS402_SW_PVM_BIT_SPEED
#define DS402_SW_PVM_BIT_SPEED
Definition: SDK_Amplifier_DS402_StateMachine.mh:114
DS402_SW_BIT_FAULT
#define DS402_SW_BIT_FAULT
Definition: SDK_Amplifier_DS402_StateMachine.mh:100
DS402_CW_PPM_BIT_ENDLESS_MOVEMENT
#define DS402_CW_PPM_BIT_ENDLESS_MOVEMENT
Definition: SDK_Amplifier_DS402_StateMachine.mh:94
DS402_OP_PPM
#define DS402_OP_PPM
Definition: SDK_Amplifier_DS402_StateMachine.mh:120
sdkDS402_PVM_WaitTargetReached
long sdkDS402_PVM_WaitTargetReached(long busId, long timeout)
Wait until target is reachd.
Definition: SDK_Amplifier_DS402_StateMachine.mc:976
DS402_PROFILE_DECELERATION
#define DS402_PROFILE_DECELERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:143
sdkDS402_PVM_CvelStart
long sdkDS402_PVM_CvelStart(long busId)
Start continuous velocity (PVM)
Definition: SDK_Amplifier_DS402_StateMachine.mc:904
sdkDS402_PPM_PosAbsStart
long sdkDS402_PPM_PosAbsStart(long busId, long pos, long startImmediately)
Start absolute positioning.
Definition: SDK_Amplifier_DS402_StateMachine.mc:797
DS402_PPM_ABSOLUTE
#define DS402_PPM_ABSOLUTE
Definition: SDK_Amplifier_DS402_StateMachine.mh:189
helperDS402_ExecuteTransition
long helperDS402_ExecuteTransition(long busId, long transition)
Execute a state transition.
Definition: SDK_Amplifier_DS402_StateMachine.mc:152
DS402_MODES_OF_OPERATION
#define DS402_MODES_OF_OPERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:119
DS402_SW_BIT_INTERNAL_LIMIT
#define DS402_SW_BIT_INTERNAL_LIMIT
Definition: SDK_Amplifier_DS402_StateMachine.mh:107
DS402_TRANSITION_DISABLE_OPERATION
#define DS402_TRANSITION_DISABLE_OPERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:74
DS402_DRIVE_STATE_OPERATION_ENABLED
#define DS402_DRIVE_STATE_OPERATION_ENABLED
Definition: SDK_Amplifier_DS402_StateMachine.mh:64
DS402_DRIVE_STATE_NOT_READY_TO_SWITCH_ON
#define DS402_DRIVE_STATE_NOT_READY_TO_SWITCH_ON
Drive States and Transitions - CiA 402 State Machine.
Definition: SDK_Amplifier_DS402_StateMachine.mh:60
DS402_DRIVE_STATE_READY_TO_SWITCH_ON
#define DS402_DRIVE_STATE_READY_TO_SWITCH_ON
Definition: SDK_Amplifier_DS402_StateMachine.mh:62
sdkDS402_PrintState
void sdkDS402_PrintState(long state)
Print the name of certain drive state.
Definition: SDK_Amplifier_DS402_StateMachine.mc:111
sdkDS402_SetProfileDeceleration
void sdkDS402_SetProfileDeceleration(long busId, long dec)
Set the profile deceleration DS402_PROFILE_DECELERATION(0x6084)
Definition: SDK_Amplifier_DS402_StateMachine.mc:401
sdkDS402_SetMaxMotorSpeed
void sdkDS402_SetMaxMotorSpeed(long busId, long vel)
Set the max profile velocity DS402_MAX_MOTOR_SPEED(0x6080)
Definition: SDK_Amplifier_DS402_StateMachine.mc:452
SDK_Amplifier_DS402_StateMachine.mh
Declaration of the DS402 StateMachineFunction.
sdkDS402_SetProfileAcceleration
void sdkDS402_SetProfileAcceleration(long busId, long acc)
Profile Mode settings.
Definition: SDK_Amplifier_DS402_StateMachine.mc:389
DS402_CW_BIT_ENABLE_VOLTAGE
#define DS402_CW_BIT_ENABLE_VOLTAGE
Definition: SDK_Amplifier_DS402_StateMachine.mh:85
DS402_SW_BIT_READY_TO_SWITCH_ON
#define DS402_SW_BIT_READY_TO_SWITCH_ON
Definition: SDK_Amplifier_DS402_StateMachine.mh:97
DS402_SW_PPM_BIT_FOLLWING_ERROR
#define DS402_SW_PPM_BIT_FOLLWING_ERROR
Definition: SDK_Amplifier_DS402_StateMachine.mh:111
sdkDS402_PPM_TargetReached
long sdkDS402_PPM_TargetReached(long busId)
Checks whether the target was reached.
Definition: SDK_Amplifier_DS402_StateMachine.mc:836
DS402_QUICK_STOP_DECELERATION
#define DS402_QUICK_STOP_DECELERATION
Definition: SDK_Amplifier_DS402_StateMachine.mh:144
sdkDS402_QuickStop
void sdkDS402_QuickStop(long busId)
Execute quick stop.
Definition: SDK_Amplifier_DS402_StateMachine.mc:544
DS402_CW_BIT_SWITCH_ON
#define DS402_CW_BIT_SWITCH_ON
Definition: SDK_Amplifier_DS402_StateMachine.mh:84
DS402_SW_PPM_BIT_SETPOINT_ACK
#define DS402_SW_PPM_BIT_SETPOINT_ACK
Definition: SDK_Amplifier_DS402_StateMachine.mh:110
DS402_CW_PPM_BIT_NEW_SETPOINT
#define DS402_CW_PPM_BIT_NEW_SETPOINT
Definition: SDK_Amplifier_DS402_StateMachine.mh:91
DS402_CONTROLWORD
#define DS402_CONTROLWORD
Definition: SDK_Amplifier_DS402_StateMachine.mh:83
DS402_OP_PVM
#define DS402_OP_PVM
Definition: SDK_Amplifier_DS402_StateMachine.mh:121
sdkDS402_SetOpartionMode
void sdkDS402_SetOpartionMode(long busId, long operationMode)
Profile Mode functions.
Definition: SDK_Amplifier_DS402_StateMachine.mc:508
DS402_TRANSITION_QUICK_STOP
#define DS402_TRANSITION_QUICK_STOP
Definition: SDK_Amplifier_DS402_StateMachine.mh:75
DS402_DRIVE_STATE_SWITCHED_ON
#define DS402_DRIVE_STATE_SWITCHED_ON
Definition: SDK_Amplifier_DS402_StateMachine.mh:63
DS402_CW_PPM_BIT_CHANGE_IMMEDIATELY
#define DS402_CW_PPM_BIT_CHANGE_IMMEDIATELY
Definition: SDK_Amplifier_DS402_StateMachine.mh:92
DS402_PPM_RELATIVE
#define DS402_PPM_RELATIVE
Definition: SDK_Amplifier_DS402_StateMachine.mh:190
sdkDS402_WaitTransitionToState
long sdkDS402_WaitTransitionToState(long busId, long targetState, long timeout)
Wait transition to another state.
Definition: SDK_Amplifier_DS402_StateMachine.mc:347
sdkDS402_SetMaxProfileVelocity
void sdkDS402_SetMaxProfileVelocity(long busId, long vel)
Set the max profile velocity DS402_MAX_PROFILE_VELOCITY(0x607F)
Definition: SDK_Amplifier_DS402_StateMachine.mc:439
sdkDS402_PVM_SetTargetVelocity
void sdkDS402_PVM_SetTargetVelocity(long busId, long targetVelocity)
Profile Velocity Mode functions.
Definition: SDK_Amplifier_DS402_StateMachine.mc:888
sdkDS402_WriteControlWord
void sdkDS402_WriteControlWord(long busId, long value)
Write control word.
Definition: SDK_Amplifier_DS402_StateMachine.mc:60
DS402_TRANSITION_SHUTDOWN
#define DS402_TRANSITION_SHUTDOWN
Definition: SDK_Amplifier_DS402_StateMachine.mh:70
sdkDS402_PVM_CvelStop
long sdkDS402_PVM_CvelStop(long busId)
Stop continuous velocity (PVM)
Definition: SDK_Amplifier_DS402_StateMachine.mc:935

Data Sheets | Released Software | Software Manuals | Hardware Manuals | Maxon Shop

Maxon Support Center