J L
ringBuffer26
Diff: writeCommands.cpp
- Revision:
- 1:0cb065f9d55a
--- /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);
+}
+
+
+
+
+