config finished except stepsperturn, mmperturn
This commit is contained in:
1 parent
65502c60f0
commit
8071f4c246
2 files changed
+291
-94
No files matched your search
@@ -23,9 +23,16 @@ static L6474_Handle_t stepperHandle = NULL;
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
int state = 0;
|
int state = 0;
|
||||||
|
float posmax = 29.5;
|
||||||
|
float posmin = 0.0f;
|
||||||
|
float posref = 0.0f;
|
||||||
|
|
||||||
|
// eingestellter Step Mode: 1, 2, 4, 8 oder 16
|
||||||
|
int microsteps = 16;
|
||||||
|
|
||||||
|
// 50 Vollschritte pro mm, bei 1/16 Schritt also 800 Schritte pro mm
|
||||||
int mm_to_step(float mm){
|
int mm_to_step(float mm){
|
||||||
return (int)(800 * mm);
|
return (int)(50 * microsteps * mm);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void* StepLibraryMalloc( unsigned int size ){
|
static void* StepLibraryMalloc( unsigned int size ){
|
||||||
@@ -71,6 +78,9 @@ static int StepperHasFault(L6474_Handle_t s){
|
|||||||
|
|
||||||
// Pulsfrequenz einstellen, nur aufrufen wenn gerade keine Pulse laufen
|
// Pulsfrequenz einstellen, nur aufrufen wenn gerade keine Pulse laufen
|
||||||
static void StepTimerSetSpeed(unsigned int hz){
|
static void StepTimerSetSpeed(unsigned int hz){
|
||||||
|
// hz gilt fuer 1/16 Schritt, bei anderem Step Mode gleiche mm/min halten
|
||||||
|
hz = hz * microsteps / 16;
|
||||||
|
|
||||||
uint32_t arr = (STEP_TICK_HZ / hz) - 1;
|
uint32_t arr = (STEP_TICK_HZ / hz) - 1;
|
||||||
|
|
||||||
stepHz = hz;
|
stepHz = hz;
|
||||||
@@ -114,15 +124,45 @@ void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim){
|
|||||||
portYIELD_FROM_ISR(woken);
|
portYIELD_FROM_ISR(woken);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Name des Config-Parameters in die Nummer fuer config_stepper wandeln, 0 wenn unbekannt
|
||||||
|
static int config_param_id(const char* name){
|
||||||
|
if(strcmp(name, "torque") == 0){
|
||||||
|
return 1;
|
||||||
|
}else if(strcmp(name, "throvercurr") == 0){
|
||||||
|
return 2;
|
||||||
|
}else if(strcmp(name, "powerena") == 0){
|
||||||
|
return 3;
|
||||||
|
}else if(strcmp(name, "stepmode") == 0){
|
||||||
|
return 4;
|
||||||
|
}else if(strcmp(name, "timeoff") == 0){
|
||||||
|
return 5;
|
||||||
|
}else if(strcmp(name, "timeon") == 0){
|
||||||
|
return 6;
|
||||||
|
}else if(strcmp(name, "timefast") == 0){
|
||||||
|
return 7;
|
||||||
|
}else if(strcmp(name, "mmperturn") == 0){
|
||||||
|
return 8;
|
||||||
|
}else if(strcmp(name, "posmax") == 0){
|
||||||
|
return 9;
|
||||||
|
}else if(strcmp(name, "posmin") == 0){
|
||||||
|
return 10;
|
||||||
|
}else if(strcmp(name, "posref") == 0){
|
||||||
|
return 11;
|
||||||
|
}else if(strcmp(name, "stepsperturn") == 0){
|
||||||
|
return 12;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
static int config_stepper(L6474_Handle_t s, int param, float value){
|
static int config_stepper(L6474_Handle_t s, int param, float value){
|
||||||
/*
|
/*
|
||||||
* param 1: torque
|
* param 1: torque (mA)
|
||||||
* param 2: throvercurr
|
* param 2: throvercurr (mA)
|
||||||
* param 3: powerena
|
* param 3: powerena (0 oder 1)
|
||||||
* param 4: stepmode
|
* param 4: stepmode (1, 2, 4, 8, 16)
|
||||||
* param 5: timeoff
|
* param 5: timeoff (us)
|
||||||
* param 6: timeon
|
* param 6: timeon (us)
|
||||||
* param 7: timefast
|
* param 7: timefast (us)
|
||||||
* param 8: mmperturn
|
* param 8: mmperturn
|
||||||
* param 9: posmax
|
* param 9: posmax
|
||||||
* param 10: posmin
|
* param 10: posmin
|
||||||
@@ -130,32 +170,171 @@ static int config_stepper(L6474_Handle_t s, int param, float value){
|
|||||||
* param 12: stepsperturn
|
* param 12: stepsperturn
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
int reg = 0;
|
||||||
|
int result = 0;
|
||||||
|
|
||||||
switch(param){
|
switch(param){
|
||||||
case 1:
|
case 1:
|
||||||
L6474_SetProperty(s, L6474_PROP_TORQUE, L6474_EncodePhaseCurrent(value));
|
result = L6474_SetProperty(s, L6474_PROP_TORQUE, L6474_EncodePhaseCurrent(value));
|
||||||
|
break;
|
||||||
case 2:
|
case 2:
|
||||||
L6474_SetProperty(s, L6474_PROP_OCDTH, value);
|
// 375 mA pro Stufe, Register 0 = 375 mA
|
||||||
//TODO value muss noch in Enum L6474x_OCD_TH_t gewandelt werden
|
if(value < 375 || value > 6000){
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
reg = (int)(value / 375) - 1;
|
||||||
|
result = L6474_SetProperty(s, L6474_PROP_OCDTH, reg);
|
||||||
|
break;
|
||||||
case 3:
|
case 3:
|
||||||
if(value == 1){
|
if(value == 1){
|
||||||
HAL_GPIO_WritePin(STEP_RSTN_GPIO_Port, STEP_RSTN_Pin, GPIO_PIN_SET);
|
result = L6474_SetPowerOutputs(s, 1);
|
||||||
}else{
|
}else{
|
||||||
HAL_GPIO_WritePin(STEP_RSTN_GPIO_Port, STEP_RSTN_Pin, GPIO_PIN_RESET);
|
result = L6474_SetPowerOutputs(s, 0);
|
||||||
}
|
}
|
||||||
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
case 5:
|
if(value == 1){
|
||||||
case 6:
|
result = L6474_SetStepMode(s, smFULL);
|
||||||
case 7:
|
}else if(value == 2){
|
||||||
case 8:
|
result = L6474_SetStepMode(s, smHALF);
|
||||||
case 9:
|
}else if(value == 4){
|
||||||
case 10:
|
result = L6474_SetStepMode(s, smMICRO4);
|
||||||
case 11:
|
}else if(value == 8){
|
||||||
case 12:
|
result = L6474_SetStepMode(s, smMICRO8);
|
||||||
default:
|
}else if(value == 16){
|
||||||
|
result = L6474_SetStepMode(s, smMICRO16);
|
||||||
|
}else{
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if(result == errcNONE){
|
||||||
|
microsteps = (int)value;
|
||||||
|
// Position stimmt nicht mehr, neue Referenzfahrt noetig
|
||||||
|
if(state == 2){
|
||||||
|
state = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
// 0.5 us pro Stufe, Register 0 = 0.5 us
|
||||||
|
if(value < 0.5f || value > 64){
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
reg = (int)(value * 2) - 1;
|
||||||
|
result = L6474_SetProperty(s, L6474_PROP_TOFF, reg);
|
||||||
|
break;
|
||||||
|
case 6:
|
||||||
|
// 0.5 us pro Stufe, Register 0 = 0.5 us
|
||||||
|
if(value < 0.5f || value > 64){
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
reg = (int)(value * 2) - 1;
|
||||||
|
result = L6474_SetProperty(s, L6474_PROP_TON, reg);
|
||||||
|
break;
|
||||||
|
case 7:
|
||||||
|
// 2 us pro Stufe, Register 0 = 2 us
|
||||||
|
// obere 4 Bit: TOFF_FAST (wird gesetzt), untere 4 Bit: FAST_STEP (bleibt)
|
||||||
|
if(value < 2 || value > 32){
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
L6474_GetProperty(s, L6474_PROP_TFAST, ®);
|
||||||
|
reg = (((int)(value / 2) - 1) << 4) | (reg & 0x0F);
|
||||||
|
result = L6474_SetProperty(s, L6474_PROP_TFAST, reg);
|
||||||
|
break;
|
||||||
|
case 9:
|
||||||
|
posmax = value;
|
||||||
|
result = errcNONE;
|
||||||
|
break;
|
||||||
|
case 10:
|
||||||
|
posmin = value;
|
||||||
|
result = errcNONE;
|
||||||
|
break;
|
||||||
|
case 11:
|
||||||
|
posref = value;
|
||||||
|
result = errcNONE;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
//TODO param 8 bis 12
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(result != errcNONE){
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int config_stepper_get(L6474_Handle_t s, int param, float* value){
|
||||||
|
//gleiche param Nummern und Einheiten wie bei config_stepper
|
||||||
|
|
||||||
|
int reg = 0;
|
||||||
|
int result = 0;
|
||||||
|
L6474x_State_t driverState;
|
||||||
|
L6474x_StepMode_t mode;
|
||||||
|
|
||||||
|
switch(param){
|
||||||
|
case 1:
|
||||||
|
result = L6474_GetProperty(s, L6474_PROP_TORQUE, ®);
|
||||||
|
*value = (reg + 1) * 31.25f;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
result = L6474_GetProperty(s, L6474_PROP_OCDTH, ®);
|
||||||
|
*value = (reg + 1) * 375;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
result = L6474_GetState(s, &driverState);
|
||||||
|
if(driverState == stENABLED){
|
||||||
|
*value = 1;
|
||||||
|
}else{
|
||||||
|
*value = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
result = L6474_GetStepMode(s, &mode);
|
||||||
|
if(mode == smFULL){
|
||||||
|
*value = 1;
|
||||||
|
}else if(mode == smHALF){
|
||||||
|
*value = 2;
|
||||||
|
}else if(mode == smMICRO4){
|
||||||
|
*value = 4;
|
||||||
|
}else if(mode == smMICRO8){
|
||||||
|
*value = 8;
|
||||||
|
}else{
|
||||||
|
*value = 16;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
result = L6474_GetProperty(s, L6474_PROP_TOFF, ®);
|
||||||
|
*value = (reg + 1) * 0.5f;
|
||||||
|
break;
|
||||||
|
case 6:
|
||||||
|
result = L6474_GetProperty(s, L6474_PROP_TON, ®);
|
||||||
|
*value = (reg + 1) * 0.5f;
|
||||||
|
break;
|
||||||
|
case 7:
|
||||||
|
result = L6474_GetProperty(s, L6474_PROP_TFAST, ®);
|
||||||
|
*value = ((reg >> 4) + 1) * 2;
|
||||||
|
break;
|
||||||
|
case 9:
|
||||||
|
*value = posmax;
|
||||||
|
result = errcNONE;
|
||||||
|
break;
|
||||||
|
case 10:
|
||||||
|
*value = posmin;
|
||||||
|
result = errcNONE;
|
||||||
|
case 11:
|
||||||
|
*value = posref;
|
||||||
|
result = errcNONE;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
//TODO param 8 bis 12
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if(result != errcNONE){
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
static int StepLibraryStep( void* pPWM, int dir, unsigned int numPulses ){
|
static int StepLibraryStep( void* pPWM, int dir, unsigned int numPulses ){
|
||||||
@@ -227,6 +406,7 @@ int reset_stepper(L6474_Handle_t s){
|
|||||||
L6474_BaseParameter_t b;
|
L6474_BaseParameter_t b;
|
||||||
L6474_SetBaseParameter(&b);
|
L6474_SetBaseParameter(&b);
|
||||||
b.stepMode = smMICRO16;
|
b.stepMode = smMICRO16;
|
||||||
|
microsteps = 16;
|
||||||
b.OcdTh = ocdth1125mA;
|
b.OcdTh = ocdth1125mA;
|
||||||
L6474_EncodePhaseCurrentParameter(&b, 700.0f);
|
L6474_EncodePhaseCurrentParameter(&b, 700.0f);
|
||||||
|
|
||||||
@@ -287,7 +467,7 @@ int stepper_reference(L6474_Handle_t s, int flag){
|
|||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
L6474_SetAbsolutePosition(s, 0);
|
L6474_SetAbsolutePosition(s, mm_to_step(posref));
|
||||||
|
|
||||||
return StepperHasFault(s);
|
return StepperHasFault(s);
|
||||||
|
|
||||||
@@ -302,7 +482,7 @@ int stepper_pos(L6474_Handle_t s){
|
|||||||
if(L6474_GetAbsolutePosition(s, &position) != errcNONE){
|
if(L6474_GetAbsolutePosition(s, &position) != errcNONE){
|
||||||
return 1;
|
return 1;
|
||||||
}
|
}
|
||||||
printf("%d", (position/800));
|
printf("%d", (position/(50 * microsteps)));
|
||||||
return StepperHasFault(s);
|
return StepperHasFault(s);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -456,7 +636,7 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx )
|
|||||||
}
|
}
|
||||||
float target_mm = (float)atof(argv[1]);
|
float target_mm = (float)atof(argv[1]);
|
||||||
|
|
||||||
if (target_mm < 0.0f || target_mm > 100.0f) {
|
if (target_mm < posmin || target_mm > posmax) {
|
||||||
printf("position out of range\r\n");
|
printf("position out of range\r\n");
|
||||||
printf("FAIL\r\n");
|
printf("FAIL\r\n");
|
||||||
return -1;
|
return -1;
|
||||||
@@ -509,10 +689,44 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx )
|
|||||||
L6474_IsMoving(h, &moving);
|
L6474_IsMoving(h, &moving);
|
||||||
|
|
||||||
|
|
||||||
printf("0x%X\r\n", (unsigned int)currentState); //print state in first line
|
//TEST CODE START
|
||||||
|
|
||||||
|
unsigned int mappedState = 0x0;
|
||||||
|
|
||||||
|
// check for error (would print 0x8)
|
||||||
|
if (errorBits != 0) {
|
||||||
|
mappedState = 0x8; // scsFLT
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
switch (currentState) {
|
||||||
|
case stRESET:
|
||||||
|
mappedState = 0x1; //map stRESET to scsREF?
|
||||||
|
break;
|
||||||
|
|
||||||
|
case stDISABLED:
|
||||||
|
mappedState = 0x2; //map stDISABLED to scsDIS (driver ready, otuput off)?
|
||||||
|
break;
|
||||||
|
|
||||||
|
case stENABLED:
|
||||||
|
mappedState = 0x4; //map stENABLED to scsENA --> driver active + output on
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
mappedState = 0x0; //entsprciht scsInit
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("0x%X\r\n", mappedState); //print state machine state in first line
|
||||||
|
|
||||||
|
//TEST CODE END
|
||||||
|
|
||||||
|
|
||||||
|
//printf("0x%X\r\n", (unsigned int)currentState); //print state in first line
|
||||||
printf("0x%X\r\n", errorBits); //print error flags in second line
|
printf("0x%X\r\n", errorBits); //print error flags in second line
|
||||||
printf("internal error bits in binary: ");
|
printf("internal error bits in binary: ");
|
||||||
print_binary_8bit(errorBits);
|
print_binary_8bit(errorBits);
|
||||||
|
|
||||||
printf("%d\r\n", moving); //print async pending op in third line
|
printf("%d\r\n", moving); //print async pending op in third line
|
||||||
printf("OK");
|
printf("OK");
|
||||||
|
|
||||||
@@ -520,67 +734,50 @@ int StepperConsoleFunction( int argc, char** argv, void* ctx )
|
|||||||
}
|
}
|
||||||
else if (strcmp(argv[0], "config") == 0)
|
else if (strcmp(argv[0], "config") == 0)
|
||||||
{
|
{
|
||||||
float value = atof(argv[2]);
|
|
||||||
if ( argc < 2 ){
|
if ( argc < 2 ){
|
||||||
|
printf("missing parameter name for config command\r\n");
|
||||||
printf("Too few arguments\r\n");
|
|
||||||
printf("FAIL\r\n");
|
printf("FAIL\r\n");
|
||||||
return 1;
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
}
|
int param = config_param_id(argv[1]);
|
||||||
switch(atoi(argv[1])){
|
|
||||||
case 1:
|
if(param == 0){
|
||||||
config_stepper(h, 1, value);
|
printf("unknown config parameter\r\n");
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 2:
|
|
||||||
config_stepper(h, 2, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 3:
|
|
||||||
config_stepper(h, 3, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 4:
|
|
||||||
config_stepper(h, 4, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 5:
|
|
||||||
config_stepper(h, 5, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 6:
|
|
||||||
config_stepper(h, 6, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 7:
|
|
||||||
config_stepper(h, 7, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 8:
|
|
||||||
config_stepper(h, 8, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 9:
|
|
||||||
config_stepper(h, 9, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 10:
|
|
||||||
config_stepper(h, 10, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 11:
|
|
||||||
config_stepper(h, 11, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
case 12:
|
|
||||||
config_stepper(h, 12, value);
|
|
||||||
printf("OK\r\n");
|
|
||||||
return 0;
|
|
||||||
default:
|
|
||||||
printf("FAIL\r\n");
|
printf("FAIL\r\n");
|
||||||
return 1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Ohne Flag: Wert lesen -> config <param>
|
||||||
|
if ( argc == 2 ){
|
||||||
|
float value = 0.0f;
|
||||||
|
|
||||||
|
if(config_stepper_get(h, param, &value) != 0){
|
||||||
|
printf("FAIL\r\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("%.2f\r\n", value);
|
||||||
|
printf("OK\r\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Mit Flag: Wert schreiben -> config <param> -v <value>
|
||||||
|
if ( argc >= 4 && strcmp(argv[2], "-v") == 0 ){
|
||||||
|
float value = (float)atof(argv[3]);
|
||||||
|
|
||||||
|
if(config_stepper(h, param, value) != 0){
|
||||||
|
printf("FAIL\r\n");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("OK\r\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("usage: config <param> [-v <value>]\r\n");
|
||||||
|
printf("FAIL\r\n");
|
||||||
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
||||||
else
|
else
|
||||||
|
|||||||
@@ -183,20 +183,20 @@ static int CapabilityFunc( int argc, char** argv, void* ctx )
|
|||||||
0, // has stepper refrun timeout
|
0, // has stepper refrun timeout
|
||||||
0, // has stepper refrun skip
|
0, // has stepper refrun skip
|
||||||
0, // has stepper refrun stay enabled
|
0, // has stepper refrun stay enabled
|
||||||
0, // has stepper reset
|
1, // has stepper reset
|
||||||
1, // has stepper position
|
1, // has stepper position
|
||||||
0, // has stepper config
|
1, // has stepper config
|
||||||
0, // has stepper config torque
|
2, // has stepper config torque
|
||||||
0, // has stepper config throvercurr
|
1, // has stepper config throvercurr
|
||||||
0, // has stepper config powerena
|
1, // has stepper config powerena
|
||||||
0, // has stepper config stepmode
|
1, // has stepper config stepmode
|
||||||
0, // has stepper config timeoff
|
1, // has stepper config timeoff
|
||||||
0, // has stepper config timeon
|
1, // has stepper config timeon
|
||||||
0, // has stepper config timefast
|
1, // has stepper config timefast
|
||||||
0, // has stepper config mmperturn
|
0, // has stepper config mmperturn
|
||||||
0, // has stepper config posmax
|
1, // has stepper config posmax
|
||||||
0, // has stepper config posmin
|
1, // has stepper config posmin
|
||||||
0, // has stepper config posref
|
1, // has stepper config posref
|
||||||
0, // has stepper config stepsperturn
|
0, // has stepper config stepsperturn
|
||||||
0 // has stepper cancel
|
0 // has stepper cancel
|
||||||
);
|
);
|
||||||
|
|||||||
Reference in new issue
Block a user