J L
/
ringBuffer26a
ringBuffer26a
Diff: writeCommands.cpp
- Revision:
- 1:0cb065f9d55a
diff -r 333434a8611b -r 0cb065f9d55a writeCommands.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/writeCommands.cpp Mon May 18 19:04:41 2020 +0000 @@ -0,0 +1,374 @@ +#include <stdio.h> +#include "mbed.h" +#include "main.h" +#include "MotorControl.h" +#include "writeCommands.h" +#include<string> + +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +// PRODUCT COMMAND FUNCTIONS +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + +bool setTorque(uint16_t newtonMilliMetres, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Set Torque +// +// newtonMilliMetres : (0 - 8500) : 0 - 8.5 N.m +// +// return : invalid param = 1, ok = 0 +// +//////////////////////////////////////////////////////////////////////////////// + + bool invParm = false; + + if(rxMsgPending == false){ + if(newtonMilliMetres <= MAX_TORQUE){ + sprintf(array,"SET FEEDBACK BOOST CURRENT %u\r",newtonMilliMetres); + mc_debug.printf("%s",array); + //mc_usart.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("Set Torque %u\r\n", newtonMilliMetres); + mc_usart.printf("Set Torque %u\r\n", newtonMilliMetres); + #endif + } + else{ + invParm = true; + #ifdef DEBUG + //mc_debug.printf("set Torque parameter fail\r\n"); + mc_usart.printf("set Torque parameter fail\r\n"); + #endif + } +/* + if(cmdCnt < COMMANDS_TO_TEST) + cmdCnt++; + else + cmdCnt=0; +*/ + //rxMsgPending = true; + led1=0; + } + return(invParm); +} + + +bool set_mL(bool direction, uint32_t mL, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Set milli litres +// +// direction : CW = 1, CCW = 0 +// +// mL : 0 - 2147483648, -2147483648 - 0 : 2.1 billion +// +// return : invalid param = 1, ok = 0 +// +//////////////////////////////////////////////////////////////////////////////// + + bool invParm = false; + uint32_t steps = 0; + + if(rxMsgPending == false){ + steps = ((mL/1000)/(ML_ROTATION/1000))*STEPS_PER_ROTATION; + + if(direction == CW) + invParm = moveMot(2, steps, array); + else + if(direction == CCW) + invParm = moveMot(3, steps, array); + + #ifdef DEBUG + if(direction == CW){ + //mc_debug.printf("Dispense CW %d steps\r\n", steps); + mc_usart.printf("Dispense CW %d steps\r\n", steps); + } + else + if(direction == CCW){ + //mc_debug.printf("Dispense CCW %d steps\r\n", steps); + mc_usart.printf("Dispense CCW %d steps\r\n", steps); + } + + #endif +/* + if(cmdCnt < COMMANDS_TO_TEST) + cmdCnt++; + else + cmdCnt=0; +*/ + //rxMsgPending = true; + led1=0; + } + return(invParm); +} + +bool setRotMotor(bool direction, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Rotate motor +// +// direction : CW = 1, CCW = 0 +// +// return : invalid param = 1, ok = 0 +// +//////////////////////////////////////////////////////////////////////////////// + + bool invParm = false; + + if(rxMsgPending == false){ + if(direction == CW) + invParm = moveMot(0, 0, array); + else + if(direction == CCW) + invParm = moveMot(1, 0, array); + + #ifdef DEBUG + if(direction == CW){ + //mc_debug.printf("MOVEMOT free run CW\r\n"); + mc_usart.printf("MOVEMOT free run CW\r\n"); + } + else + if(direction == CCW){ + //mc_debug.printf("MOVEMOT free run CCW\r\n"); + mc_usart.printf("MOVEMOT free run CCW\r\n"); + } + + #endif +/* + if(cmdCnt < COMMANDS_TO_TEST) + cmdCnt++; + else + cmdCnt=0; +*/ + //rxMsgPending = true; + led1=0; + } + return(invParm); +} + +bool stopMot(uint8_t par1, int32_t par2, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Stop Motor +// +// par1 = 0 : stop with no ramp | par2 : not used +// par1 = 1 : stop with ramp | par2 : not used +// par1 = 2 : stop with steps | par2 : number of stop steps +// +// return : invalid param = 1 +// +//////////////////////////////////////////////////////////////////////////////// + + static uint8_t newPar1 = 0xff; + static int32_t newPar2 = 0xff; + bool invParm = false; + + //if((par1 != newPar1) || (par2 != newPar2)){ + + newPar1 = par1; + newPar2 = par2; + + if(((par1 == STOP_NO_RAMP)||(par1 == STOP_WITH_RAMP))&&(par2 == NOT_USED)){ + sprintf(array,"STOPMOT %u %u\r",par1, par2); + mc_debug.printf("%s",array); + //mc_usart.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("STOPMOT %u %u\r",par1, par2); + mc_usart.printf("STOPMOT %u %u\r",par1, par2); + #endif + } + else + if((par1 == STOP_WITH_STEPS)&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){ + sprintf(array,"STOPMOT %u %u\r",par1, par2); + mc_debug.printf("%s",array); + //mc_usart.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("STOPMOT %u %u\r",par1, par2); + mc_usart.printf("STOPMOT %u %u\r",par1, par2); + #endif + } + else{ + invParm = true; + #ifdef DEBUG + //mc_debug.printf("STOPMOT parameter fail\r\n"); + mc_usart.printf("STOPMOT parameter fail\r\n"); + #endif + } + //} + led1=0; + + return(invParm); +} + +bool setSpeed(float *n_speed, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Set Speed +// +// 1000 = 0.001 RPM +// 3000000 = 125.0 RPM +// +// range 1000 - 3000000 +// +// speed :(1 - 125000) : 0.001 - 125.000 RPM +// +// return : invalid param = 1, ok = 0 +// +//////////////////////////////////////////////////////////////////////////////// + + bool invParm = false; + uint32_t ed_speed;//motor controller speed, 1000 - 3000000 + + if((*n_speed >= N_MIN_SPEED)&&(*n_speed <= N_MAX_SPEED)){ + + ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED); + + sprintf(array,"SET PROFILE VELOCITY %u\r",ed_speed); + mc_debug.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("ED_SetSpeed %u, Neptune_SetSpeed %f\r\n", ed_speed, n_speed); + mc_usart.printf("ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); + #endif + + led1=0; + } + else{ + + if(*n_speed > N_MAX_SPEED) + *n_speed = N_MAX_SPEED; + else + if(*n_speed < N_MIN_SPEED) + *n_speed = N_MIN_SPEED; + + +/* + #ifdef DEBUG + ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED); + //mc_debug.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); + mc_usart.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); + #endif +*/ + } + return(invParm); +} + +bool setWorkSetExt(uint16_t par1, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Set Working Settings Extended +// +// par1 : bit 5 = disable auto motor enable +// par1 : bit 6 = encoder rotation direction +// par1 : bit 9 = RL motor detection +// par1 : bit 10 = enable I2T protection +// +// return : invalid param = 1, ok = 0 +// +//////////////////////////////////////////////////////////////////////////////// + + static uint8_t newPar1 = 0xff; + bool invParm = false; + + if(rxMsgPending == false){ + if(par1 != newPar1){ + + newPar1 = par1; + + if(par1 <= 15){ + sprintf(array,"SET DRIVE WORKING SETTINGS EXTENDED %u\r",par1); + mc_debug.printf("%s",array); + //mc_usart.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("Drive working setting extended %u\r\n", par1); + mc_usart.printf("Drive working setting extended %u\r\n", par1); + #endif + } + else{ + invParm = true; + #ifdef DEBUG + //mc_debug.printf("setWorkSet parameter fail\r\n"); + mc_usart.printf("setWorkSet parameter fail\r\n"); + #endif + } + } +/* + if(cmdCnt < COMMANDS_TO_TEST) + cmdCnt++; + else + cmdCnt=0; +*/ + //rxMsgPending = true; + led1=0; + } + return(invParm); +} + +bool moveMot(uint8_t par1, int32_t par2, char *array){ +//////////////////////////////////////////////////////////////////////////////// +// Move Motor +// +// par1 = 0 : move free running forward | par2 : not used +// par1 = 1 : move free running backwards | par2 : not used +// par1 = 2 : move step forward | par2 : number of steps +// par1 = 3 : move step backwards | par2 : number of steps +// par1 - 4 : move to target posistion | par2 : posistion to reach +// +// return : invalid param = 1, ok = 0 +// +//////////////////////////////////////////////////////////////////////////////// + + static uint8_t newPar1 = 0xff; + static int32_t newPar2 = 0xff; + bool invParm = false; + + if(rxMsgPending == false){ + if((par1 != newPar1) || (par2 != newPar2)){ + + newPar1 = par1; + newPar2 = par2; + + if(((par1 >=2)&&(par1 <=4))&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){ + sprintf(array,"MOVEMOT %u %u\r",par1, par2); + mc_debug.printf("%s",array); + //mc_usart.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2); + mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2); + #endif + } + else + if((par1 == 0)||(par1 ==1)){ + sprintf(array,"MOVEMOT %u %u\r",par1, 0); + mc_debug.printf("%s\r",array); + mc_usart.printf("%s",array); + + #ifdef DEBUG + //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2); + mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2); + #endif + } + else{ + invParm = true; + #ifdef DEBUG + mc_debug.printf("movMot parameter fail\r\n"); + mc_usart.printf("movMot parameter fail\r\n"); + #endif + } + } +/* + if(cmdCnt < COMMANDS_TO_TEST) + cmdCnt++; + else + cmdCnt=0; +*/ + //rxMsgPending = true; + led1=0; + } + return(invParm); +} + + + + +