J L
/
ringBuffer26a
ringBuffer26a
writeCommands.cpp
- Committer:
- Picmon
- Date:
- 2020-05-18
- Revision:
- 1:0cb065f9d55a
File content as of revision 1:0cb065f9d55a:
#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); }