J L
ringBuffer26
Revision 1:0cb065f9d55a, committed 2020-05-18
- Comitter:
- Picmon
- Date:
- Mon May 18 19:04:41 2020 +0000
- Parent:
- 0:333434a8611b
- Commit message:
- ring buffer26;
Changed in this revision
diff -r 333434a8611b -r 0cb065f9d55a MotorControl.cpp
--- a/MotorControl.cpp Fri Mar 20 10:54:58 2020 +0000
+++ b/MotorControl.cpp Mon May 18 19:04:41 2020 +0000
@@ -1,149 +1,465 @@
-#include "mbed.h"
-#include "MotorControl.h"
+/*
+
+EOL SEQUENCE = NONE ON YAT, PROTOCOL WILL FAIL IF THIS IS NOT DONE
-RawSerial mc_usart(PA_2,PA_15,115200);// this will be USART6 for motor control
+*/
+
+
+
-const string rxMsgTable[MESSAGE] = {
+#include <stdio.h>
+#include "mbed.h"
+#include "main.h"
+#include "MotorControl.h"
+#include<string>
- "SET DRIVE WORKING SETINGS",
- "SET FEEDBACK BOOST CURRENT",
+class motorControl{
+
+ private:
+
+ public:
+
+ protected:
};
-struct drvWorkingSettings{
-
- bool par0;
- bool par1;
- bool par2;
- bool par3;
- bool feedBackMotChk;
- bool par5;
- bool auomaticMotCurRed;
- bool disableDigOutputsFWH;
- bool par8;
- bool par9;
- bool motRotDir;
- bool par11;
- bool par12;
- bool fbErrMotAct;
- bool par14;
- bool par15;
-
-};
+char *ptr;
+
+volatile uint8_t count = 0;
+
+bool msgValid = false;
+bool msgRdy = false;
+
+uint8_t i=0;
+
+DigitalIn d3_button(D3_BUTTON);
+DigitalIn d4_button(D4_BUTTON);
+DigitalIn d5_button(D5_BUTTON);
+DigitalIn d6_button(D6_BUTTON);
+DigitalIn d7_button(D7_BUTTON);
+DigitalIn d8_button(D8_BUTTON);
+DigitalIn d9_button(D9_BUTTON);
+DigitalIn d10_button(D10_BUTTON);
+DigitalIn d11_button(D11_BUTTON);
+DigitalIn d12_button(D12_BUTTON);
+
+DigitalOut a7_out1(A7_OUT1);//mot enable
+DigitalOut a6_out2(A6_OUT2);
+DigitalOut a5_out3(A5_OUT3);
-char rxMsgStore[BUFFER_SIZE];//received local message store
+DigitalIn a4_in1(A4_IN1);
+DigitalIn a3_in2(A3_IN2);
+DigitalIn a2_in3(A2_IN3);
+
+DigitalOut a1_out(A1_OUT);
+DigitalOut a0_out(A0_OUT);
+
+
+char rxMsgStore[BUFFER_SIZE];//received message store
char mc_Tx_Buffer[BUFFER_SIZE];//transmitted message buffer
-volatile char mc_Rx_Buffer[BUFFER_SIZE];//received message buffer
-volatile unsigned char mc_Rx_Rd_Ptr = 0;//circulare buffer read pointer
-volatile unsigned char mc_Rx_Wr_Ptr = 0;//circulare buffer write pointer
-unsigned int rx_int = 0;//received integer / character
+char mc_Rx_Buffer[BUFFER_SIZE];//received message buffer
+volatile unsigned char mc_Rx_Rd_Ptr = 0;//circular buffer read pointer
+volatile unsigned char mc_Rx_Wr_Ptr = 0;//circular buffer write pointer
+volatile bool rxMsgValid = false;//message received flag
+volatile bool rxMsgInvalid = false;//message invalid flag
-//unsigned char i;
-bool msgRecFlag = false;//message received flag
+char motData[DATA_ITEMS][DATA_BUFFER_SIZE];//data only
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// MOTOR ISR FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void motMsg_RX_ISR(void){
- //led1=1;
- //save character in buffer
- mc_Rx_Buffer[mc_Rx_Wr_Ptr] = mc_usart.getc();
- //increment pointer
- mc_Rx_Wr_Ptr++;
- //test for wrap around
- if(mc_Rx_Wr_Ptr == (BUFFER_SIZE - 1))
- {
- mc_Rx_Wr_Ptr = 0;
- }
- //wait(0.0001);
- //led1=0;
-}
-
-bool getMotMsg(char * msgArray){
-
- //declare variables
- static unsigned char i;
- unsigned int uiMsg= 0xFFFF;
+ mc_Rx_Buffer[mc_Rx_Wr_Ptr] = mc_debug.getc();
- //test for any characters in the receiver buffer
- if (mc_Rx_Wr_Ptr != mc_Rx_Rd_Ptr)
- {
- //character in the buffer, return the value
- uiMsg = mc_Rx_Buffer[mc_Rx_Rd_Ptr];
- //increment the Rd pointer
- mc_Rx_Rd_Ptr++;
- //test for wrap around
- if(mc_Rx_Rd_Ptr == (BUFFER_SIZE - 1))
- {
- mc_Rx_Rd_Ptr = 0;
- }
- }
-
- if(uiMsg != 0xFFFF){//check that a character is in the buffer
- msgArray[i++] = uiMsg;//store characters in local message store array
+ mc_Rx_Wr_Ptr++;
+ count++;
+
+ if(mc_Rx_Wr_Ptr == (BUFFER_SIZE - 1))
+ mc_Rx_Wr_Ptr = 0;
- mc_usart.printf("%c\r\n", uiMsg);
-
- if(uiMsg == '\r'){//look for <CR> then print out the message in the RX buffer
-
- msgRecFlag = true;//set the message received flag
-
- mc_usart.printf("Stored Message\r\n");
-
- for(i=0 ; i <BUFFER_SIZE ; i++)
- mc_usart.printf("%c\r\n",msgArray[i]);
-
- }
- }
- else
- i=0;
-
-
- return(msgRecFlag);
-}
-
-void motMsgDecode(unsigned int rxMsgTable[], unsigned int rxMsg[]){
-
- if(memcmp(rxMsgTable, rxMsg, 10)){
-
- }
-
}
-void clrRxMsg(char array[]){
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// MOTOR READ FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+uint8_t getMotMsg(char *txArray, char *rxArray){
+////////////////////////////////////////////////////////////////////////////////
+// get the motor message
+//
+// funtion receives command messages eched from the motor controller, the
+// message eched is checked and verified, if correct the message is parsed
+// to expose the numerical data, this data is stored in the relevant memory
+// location for later use
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+ uint8_t msgStatus = NULL;
+ static uint8_t msgReTry = 0;
+
+ while(mc_Rx_Wr_Ptr != mc_Rx_Rd_Ptr){// if not true we have a message to collect
+
+ rxArray[i] = mc_Rx_Buffer[mc_Rx_Rd_Ptr];//write the charcter into the array element indexed by i
- for(unsigned char i=0; i<BUFFER_SIZE; i++)
- array[i]=0;
+ if(rxArray[i] == ACK)//read the character from the array elecment indexed by i and if it is <ACK> set message valid flag (msgValid)
+ msgValid = true;
+
+ #ifdef DEBUG
+ //mc_debug.printf("%c", rxArray[i]);//print out chahacters from array indexed by i
+ //mc_usart.printf("%c", rxArray[i]);//print out chahacters from array indexed by i
+ #endif
+
+ i++;//increment the array index
+
+ mc_Rx_Rd_Ptr++;//increment the read pointer
+
+ count--;
+
+ if(mc_Rx_Rd_Ptr == (BUFFER_SIZE - 1))
+ mc_Rx_Rd_Ptr = 0;
+ }
+
+ if((mc_Rx_Wr_Ptr == mc_Rx_Rd_Ptr)&&(msgValid == true)){//full message received
+
+ msgValid = false;
+
+ if(strncmp(txArray, rxArray, strlen(txArray))==0){
+ #ifdef DEBUG
+ //mc_debug.printf("\r\nEcho compare is Successful, txArray = %s, rxArray = %s\r\n", txArray, rxArray);
+ mc_usart.printf("\r\nEcho compare is Successful, txArray = %s, rxArray = %s\r\n", txArray, rxArray);
+ #endif
+
+ msgStatus = RX_ECHO_VALID;
+ msgReTry=0;
+
+ //READ COMMAND TYPE 1 : <COMMAND><CR><VALUE><ACK>
+ if(strncmp(rxArray,"READ DIGITAL INPUTS\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DIG_IN, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("\r\nArray Contents %s\r\n\r\n", motData[READ_DIG_IN]);
+ mc_usart.printf("\r\nArray Contents %s\r\n\r\n", motData[READ_DIG_IN]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DIGITAL OUTPUTS\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DIG_OUT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DIG_OUT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DIG_OUT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ CURRENT ACTUAL\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_CURRENT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_CURRENT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_CURRENT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DRIVE REGISTER\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DRV_REG, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DRIVE REGISTER EXTENDED\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DRV_REG_EXT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG_EXT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG_EXT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DRIVE TEMPERATURE\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DRV_TEMP, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_TEMP]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_TEMP]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DRIVE VOLTAGE\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DRV_VOLTAGE, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_VOLTAGE]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_VOLTAGE]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DRIVE WORKING SETTINGS\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DRV_WORKING_SET, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ DRIVE WORKING SETTINGS EXTENDED\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_DRV_WORKING_SET_EXT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET_EXT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET_EXT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ ERROR REGISTER\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_ERROR_REG, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_ERROR_REG]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_ERROR_REG]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ FEEDBACK BOOST CURRENT\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_FB_BOOST_CUR, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FB_BOOST_CUR]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FB_BOOST_CUR]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ FEEDBACK STATUS\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_FB_STATUS, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FB_STATUS]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FB_STATUS]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ FIRMWARE VERSION\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_FW_VERSION, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FW_VERSION]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FW_VERSION]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ FIRMWARE CHECKSUM\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_FW_CHECKSUM, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FW_CHECKSUM]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FW_CHECKSUM]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ MASTER REGISTER\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_MASTER_REG, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MASTER_REG]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MASTER_REG]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ MIN CURRENT\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_MIN_CURRENT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MIN_CURRENT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MIN_CURRENT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ MAX CURRENT\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_MAX_CURRENT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MAX_CURRENT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MAX_CURRENT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ BOOST CURRENT\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_BOOST_CURRENT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_BOOST_CURRENT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_BOOST_CURRENT]);
+ #endif
+ }
+ else
+ if(strncmp(rxArray,"READ NOMINAL CURRENT\r", strlen(txArray))==0){
+ led1=1;
+ //msgStatus = RX_ECHO_VALID;
+ //msgReTry=0;
+ data2mem(READ_NOM_CURRENT, motData, rxArray);
+ #ifdef DEBUG
+ //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_NOM_CURRENT]);
+ mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_NOM_CURRENT]);
+ #endif
+ }
+ }
+ else{
- #ifdef DEBUG
- mc_usart.printf("Clear the RX local message store\r\n");
- #endif
+ msgStatus = RX_ECHO_FAIL;
+
+ if(msgReTry < MSG_RETRY_COUNT){//limit message transmit re-tries
+ mc_debug.printf("%s",txArray);//transmit the last message again
+ msgReTry++;//increment the re-try counter
- for(unsigned char i=0; i<BUFFER_SIZE; i++){
-
- #ifdef DEBUG
- mc_usart.printf("%c\r\n",array[i]);
- #endif
+ #ifdef DEBUG
+ //mc_usart.printf("\r\nEcho compare FAILED, txArray = %s, rxArray = %s\r\n", txArray, rxArray);
+ mc_usart.printf("\r\nTransmit re-try = %d, txArray = %s, rxArray = %s, txArray Length = %d\r\n", msgReTry , txArray, rxArray, strlen(txArray));
+ #endif
+ }
+ else{
+ #ifdef DEBUG
+ //mc_debug.printf("\r\nEcho compare has FAILED, txArray = %s, rxArray = %s, txArray Length = %d\r\n", txArray, rxArray, strlen(txArray));
+ mc_usart.printf("\r\nEcho compare has FAILED, re-try = %d, txArray = %s, rxArray = %s, txArray Length = %d\r\n", msgReTry, txArray, rxArray, strlen(txArray));
+ #endif
+ msgStatus = REBOOT;
+ msgReTry=0;
+ }
+ }
+
+ i=0;//reset the array indexer for next message
+ memset(rxArray, NULL, BUFFER_SIZE);//clear the entire buffer for next message
}
+ return(msgStatus);
}
-bool moveMot(uint8_t par1, int32_t par2){
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// mc_debug FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+void data2mem(uint8_t dataLoc, char destMemArray[][DATA_BUFFER_SIZE], char sourceMemArray[BUFFER_SIZE]){
+////////////////////////////////////////////////////////////////////////////////
+// data to memory for command type 1
+//
+//
+//
+////////////////////////////////////////////////////////////////////////////////
+
+ //parse the string to isolate the VALUE
+ //<COMMAND><CR><VALUE><ACK>
+
+ ptr = strrchr(sourceMemArray, ACK);//first set a pointer the last <ACK> address in the string,<ACK> is not required and we want this location for null terminator
- static uint8_t newPar1 = 0;
- static int32_t newPar2 = 0;
- bool fault = false;
+ *ptr = '\0';//overwrite <ACK> character with null terminator character, this will serve as end of string terminataor now
+
+ ptr = strchr(sourceMemArray, '\r');//now set the pointer first <CR> address in the string
+
+ *ptr++;//point to first character of VALUE (MSB) in the string, <CR> + 1 = <VALUE>
+
+ strcpy(destMemArray[dataLoc], ptr);//copy the <VALUE> to the destination memory location
+}
+
+void Init(void){
+
+ mc_debug.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
+ //mc_usart.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
+ led1=0;
+
+
+ mc_usart.printf("\r\n ED Motor Controler API");
+ mc_usart.printf("\r\n\r\n HARDWARE \t\t= %s", HARDWARE.c_str());
+ mc_usart.printf("\r\n\r\n SOFTWARE \t\t= %s",SOFTWARE.c_str());
+ mc_usart.printf("\r\n\r\n AUTHOR \t\t= %s",AUTHOR.c_str());
+ mc_usart.printf("\r\n\r\n DATE \t\t\t= %s",__DATE__);
+ mc_usart.printf("\r\n\r\n TIME \t\t\t= %s\r\n\r\n",__TIME__);
- if((par1 != newPar1) || (par2 != newPar2)){
-
- newPar1 = par1;
- newPar2 = par2;
-
- if(((par1 >=0) && (par1 <=4))&&((par2 >= REV_MAX_STEPS) && (par2 <= FWD_MAX_STEPS))){
- sprintf(mc_Tx_Buffer,"MOVEMOT %d %u\r",par1, par2);
- mc_usart.printf("%s\r",mc_Tx_Buffer);
- }
- else
- fault = true;
-
- }
- return(fault);
+
+/*
+ d3_button.mode(PullUp);
+ d4_button.mode(PullUp);
+ d5_button.mode(PullUp);
+ d6_button.mode(PullUp);
+ d7_button.mode(PullUp);
+ d8_button.mode(PullUp);
+ d9_button.mode(PullUp);
+ d10_button.mode(PullUp);
+ d11_button.mode(PullUp);
+ d12_button.mode(PullUp);
+*/
+
+ a7_out1 = 0;//motor enable
+ a6_out2 = 0;
+ a5_out3 = 0;
+
}
+
+long mapI(long x, long in_min, long in_max, long out_min, long out_max){
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+float mapF(float in, float inMin, float inMax, float outMin, float outMax){
+ // check it's within the range
+ if (inMin<inMax) {
+ if (in <= inMin)
+ return outMin;
+ if (in >= inMax)
+ return outMax;
+ } else { // cope with input range being backwards.
+ if (in >= inMin)
+ return outMin;
+ if (in <= inMax)
+ return outMax;
+ }
+ // calculate how far into the range we are
+ float scale = (in-inMin)/(inMax-inMin);
+ // calculate the output.
+ return outMin + scale*(outMax-outMin);
+}
\ No newline at end of file
diff -r 333434a8611b -r 0cb065f9d55a MotorControl.h --- a/MotorControl.h Fri Mar 20 10:54:58 2020 +0000 +++ b/MotorControl.h Mon May 18 19:04:41 2020 +0000 @@ -4,33 +4,197 @@ #include "mbed.h" #include <string> -#define DEBUG_MOTOR +#define DEBUG + + +#define ED_MIN_SPEED 1000 +#define ED_MAX_SPEED 3000000 +#define N_MIN_SPEED 0.001 +#define N_MAX_SPEED 125.000//125.0RPM + +#define REV_MAX_STEPS -2147483648 +#define FWD_MAX_STEPS 2147483648 + +#define MIN_CURRENT 0 +#define MAX_CURRENT 6000//6000mA + +#define MIN_TORQUE 0 +#define MAX_TORQUE 8500//N.m + + +#define SPACE 0x20 +#define ACK 0x06 +#define NAK 0x15 + +#define VALID_MSG_LEN 0x03 + +#define ML_ROTATION 5.6 +#define STEPS_PER_ROTATION 1024 + +#define CW 1 +#define CCW 0 + +#define RX_ECHO_VALID 0 +#define RX_ECHO_FAIL 1 +#define RX_REGITSER_FAIL 2 +#define RX_REGITSER_VALID 3 +#define MSG_RESEND 4 -#define BUFFER_SIZE 32 -#define MESSAGE 20 +#define REBOOT 5 + +#define MSG_RETRY_COUNT 3 + +#define NOT_USED 0 +#define STOP_NO_RAMP 0 +#define STOP_WITH_RAMP 1 +#define STOP_WITH_STEPS 2 + +#define BUFFER_SIZE 50//command message + data +#define DATA_BUFFER_SIZE 20//characters +#define DATA_ITEMS 19 + +//////////////////////////////////////////// +// MOTOR COMMAND RESPONSE DATA STORAGE +/////////////////////////////////////////// +#define READ_DIG_IN 0 +#define READ_DIG_OUT 1 +#define READ_CURRENT 2 +#define READ_DRV_REG 3 +#define READ_DRV_REG_EXT 4 +#define READ_DRV_TEMP 5 +#define READ_DRV_VOLTAGE 6 +#define READ_DRV_WORKING_SET 7 +#define READ_DRV_WORKING_SET_EXT 8 +#define READ_ERROR_REG 9 +#define READ_FB_BOOST_CUR 10 +#define READ_FB_STATUS 11 +#define READ_FW_VERSION 12 +#define READ_FW_CHECKSUM 13 +#define READ_MASTER_REG 14 +#define READ_MIN_CURRENT 15 +#define READ_MAX_CURRENT 16 +#define READ_BOOST_CURRENT 17 +#define READ_NOM_CURRENT 18 + +#define COMMANDS_TO_TEST 10 -#define REV_MAX_STEPS -2147483648 -#define FWD_MAX_STEPS 2147483648 -#define DEBUG + +//////////////////////////////////////////// +// BAUD RATES +/////////////////////////////////////////// +#define SLOW_BAUD 9600 +#define MED_BAUD 115200 +#define FAST_BAUD 921600 + +#define MOT_AL_PDT 0x0001//Protection Drive Thermal +#define MOT_AL_PDV 0x0002//Protection Drive Voltage +#define MOT_AL_PDC 0x0004//Protection Drive Current +#define MOT_AL_POP 0x0008//Protection Open Phase +#define MOT_AL_UNUSED1 0x0010//NOT USED +#define MOT_AL_FE 0x0020//Feedback Error +#define MOT_AL_PCR 0x0040//Protection Current Regulation +#define MOT_AL_POT 0x0080//Protection Open Transistor +#define MOT_AL_UNUSED2 0x0100//NOT USED +#define MOT_AL_SWER 0x0200//Protection Software Error +#define MOT_AL_UNUSED3 0x0400//NOT USED +#define MOT_AL_MC 0x0800//Missing Calibration +#define MOT_AL_WDG 0x1000//Watchdog +#define MOT_AL_EEF 0x2000//EEPROM Fail +#define MOT_AL_I2T 0x4000//I2T Potection +#define MOT_AL_UNUSED4 0x8000//NOT USED + +#define D3_BUTTON D3 +#define D4_BUTTON D4 +#define D5_BUTTON D5 +#define D6_BUTTON D6 +#define D7_BUTTON D7 +#define D8_BUTTON D8 +#define D9_BUTTON D9 +#define D10_BUTTON D10 +#define D11_BUTTON D11 +#define D12_BUTTON D12 -extern RawSerial mc_usart;// this will be USART6 for motor control +#define A7_OUT1 A7 +#define A6_OUT2 A6 +#define A5_OUT3 A5 + +#define A4_IN1 A4 +#define A3_IN2 A3 +#define A2_IN3 A2 + +#define A1_OUT A1 +#define A0_OUT A0 + +extern DigitalIn d3_button; +extern DigitalIn d4_button; +extern DigitalIn d5_button; +extern DigitalIn d6_button; +extern DigitalIn d7_button; +extern DigitalIn d8_button; +extern DigitalIn d9_button; +extern DigitalIn d10_button; +extern DigitalIn d11_button; +extern DigitalIn d12_button; + +extern DigitalOut a7_out1; +extern DigitalOut a6_out2; +extern DigitalOut a5_out3; +extern DigitalIn a4_in1; +extern DigitalIn a3_in2; +extern DigitalIn a2_in3; + +extern DigitalOut a1_out; +extern DigitalOut a0_out; + +extern char motData[][DATA_BUFFER_SIZE];//data only + +extern char binBuffer[]; +extern uint8_t cnt; extern const string rxMsgTable[]; extern char rxMsgStore[];//received local message store extern char mc_Tx_Buffer[];//transmitted message buffer -extern volatile char mc_Rx_Buffer[];//received message buffer +extern char mc_Rx_Buffer[];//received message buffer extern volatile unsigned char mc_Rx_Rd_Ptr;//circulare buffer read pointer extern volatile unsigned char mc_Rx_Wr_Ptr;//circulare buffer write pointer extern unsigned int rx_int;//received integer / character +extern volatile bool rxMsgValid;//message received flag +extern bool msgRdy; -//unsigned char i; -extern bool msgRecFlag;//message received flag +void motEncoderISR(void); + +void int2bin(int value, char* buffer, int bufferSize); +void rstPointers(void); + +void readCurrent(char *array); +bool setDigOutput(uint8_t par1, char *array); +void readDigInput(char *array); +void clrRxMsg(char *array); void motMsg_RX_ISR(void); -bool getMotMsg(char * msgArray); -void motMsgDecode(unsigned int rxMsgTable[], unsigned int rxMsg[]);void clrRxMsg(char array[]); -bool moveMot(uint8_t par1, int32_t par2); +uint8_t getMotMsg(char *txReadCmd, char *rxRegArray); + +bool set_mL(bool direction, uint32_t mL, char *array); +bool setRotMotor(bool direction, char *array); + +void initBuffer(string s, char *array, char character, uint8_t size); + +void dispBuffer(string s, uint8_t size, char array[]); +void dispMotReg(string s, uint8_t regSel, char regArray[9][BUFFER_SIZE]); + + +uint8_t procMotMsg(char *txMsg, char *rxMsg, char motReg[9][BUFFER_SIZE]); +void dispRegDec(string s, uint8_t regSel, char regArray[9][BUFFER_SIZE]); +void dispRegHex(string s, uint8_t regSel, char regArray[9][BUFFER_SIZE]); + + +void data2mem(uint8_t dataLoc, char destMemArray[][DATA_BUFFER_SIZE], char sourceMemArray[BUFFER_SIZE]);//for command with <CR><VALUE> +void Init(void); + +long mapI(long x, long in_min, long in_max, long out_min, long out_max); +float mapF(float in, float inMin, float inMax, float outMin, float outMax); + #endif \ No newline at end of file
diff -r 333434a8611b -r 0cb065f9d55a main.cpp
--- a/main.cpp Fri Mar 20 10:54:58 2020 +0000
+++ b/main.cpp Mon May 18 19:04:41 2020 +0000
@@ -1,3 +1,44 @@
+/*
+
+EOL SEQUENCE = NONE ON YAT, PROTOCOL WILL FAIL IF THIS IS NOT DONE
+
+*/
+
+
+#include "mbed.h"
+#include <string>
+#include "MotorControl.h"
+#include "readCommands.h"
+#include "writeCommands.h"
+#include "main.h"
+
+//================================================================================================================================
+// COMPANY : Verderflex
+//
+// PROJECT : ED Motor Controler API
+// DATE : 11th May 2020
+
+string HARDWARE = "Hardware Version : X1.00";
+string SOFTWARE = "Software Version : X1.00";
+string AUTHOR = "Author : Jim Lowe";
+
+/*
+ * Firmware Numbering Scheme, V Major.Minor.Bugfix
+ *
+ * Major : Major Changes to function
+ * Minor : Minor Changes to function
+ * Bugfix : Firmware fixes to function
+ *
+ */
+
+// HARDWARE : NUCLEO-F746ZG + Nextion 4.3" Display
+//
+// COMPILER : Mbed Studio
+
+//================================================================================================================================
+//************************************************ Code Status / Changes ********************************************************
+//================================================================================================================================
+
/*
@@ -10,46 +51,192 @@
In order to compare what was sent to what is echoed back two buffers are required, a TX buffer and an RX buffer, they can then be compared
using memcmp() function
- 19/03/20 Compare of tx and rx buffers can be done
- 20/03/20 Migrate code to mbed studio
-
-
+ 19/03/20 Compare of tx and rx buffers can be done
+ 20/03/20 Migrate code to mbed studio
+ 20/03/20 Code cannot be migrated to mbed studio as the target does not use mbed OS 5
+ 23/03/20 memcmp() and memcpy() implemented and working, comparision of tx and rx buffers can be done
+ 26/03/20 setSpeed(),setTorque(),setRotMotor(),set_mL() write commands tested and working with echo compare
+ 26/03/20_D ACK and NAK added, rx message checked for <CR><ACK> or <NAK>
+ 27/03/20_D Sorting of received massages into register type, echo type and NAK seems to be working
+ 30/03/20_B Process Motor Message function created that combines all message types that can be transmitted and received
+ 31/03/20_D LED1=1 indication validates data automatically, if data is invalid LED1=0
+ 03/04/20_A Setup another serial port required for testing API, one for Debug and the other to communicate with Sagittarius drive
+ 07/04/20_A Simplified so that RX ISR collects the data and stores in the required memory location
+ 08/04/20_B Software manual IMD11 V00R02 changes need to be added to software, this is a lot of work as we now have to collect a lot of bytes returnd from the motor controller
+ 11/05/20_A Back to work,
+*/
+//What is sent is received exactly, no problems
-*/
+RawSerial mc_debug(USBTX, USBRX, MED_BAUD);//debug port PA2_TX, PA15_RX
+RawSerial mc_usart(PA_9,PA_10,MED_BAUD);//motor controller communication port
+//RawSerial mc_debug(PA_2,PB_4,MED_BAUD);//motor controller communication port
-#include "mbed.h"
-#include <string>
-#include "MotorControl.h"
+//RawSerial mc_debug(PA_9,PA_10,MED_BAUD);//motor controller communication port
+//RawSerial mc_usart(USBTX, USBRX, MED_BAUD);//debug port PA2_TX, PA15_RX
+
+Timer tBtn;
DigitalOut led1(LED1);
+Timer t1;
+uint8_t msgResend = false;
+
+float n_speed = 0.001;
+
+uint8_t cmdCnt=0;
+bool rxMsgPending = false;
+
+uint32_t speed=0;
-int main(){
-
- led1=0;
+int main(){//1
+
+ Init();
+
+ //setup motor control
+ a7_out1 = 1;//motor hardware enable
+ setTorque(3456, mc_Tx_Buffer);//set torque 3.456N.m
+ setRotMotor(CCW, mc_Tx_Buffer);//set motor CCW rotation
+ n_speed = 123.456;
+ setSpeed(&n_speed, mc_Tx_Buffer);
- mc_usart.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
- mc_usart.printf("ED Motor Controller Commands\r\n");
-
- clrRxMsg(rxMsgStore);
-
- moveMot(1, 1000);
+ tBtn.start();
- mc_usart.printf("TX Buffer Contents\r\n");
+ t1.start();
+
+ while(1){//2
- for(unsigned char i=0 ; i <BUFFER_SIZE ; i++)
- mc_usart.printf("%c\r\n",mc_Tx_Buffer[i]);
-
- while(1){
+ /*identify the last message sent
+
+ the mc_Tx_Buffer contain the last message sent out which has failed
+ this message need to be resent a number of times (3 tries)
- if(getMotMsg(rxMsgStore)){//get the message from the receive buffer and store it in local message store
+ */
+
+ if(tBtn.read() > 0.100){//every x read the buttons
+
+ tBtn.reset();
+
+ if(d3_button){//speed inc
+ if(t1.read() > 1.0){
+ t1.reset();
+ n_speed+=10;
+
+ }
+ n_speed+=0.001;
+ setSpeed(&n_speed, mc_Tx_Buffer);
+ }
+ else
+ if(d4_button){//speed dec
+ if(t1.read() > 1.0){
+ t1.reset();
+ n_speed-=10;
+ }
+ n_speed-=0.001;
+ setSpeed(&n_speed, mc_Tx_Buffer);
+ }
+ else
+ if(d5_button){//stop
+ stopMot(STOP_NO_RAMP, NOT_USED, mc_Tx_Buffer);
+ }
+ else
+ if(d6_button){
+
+
+ }
+ else
+ if(d7_button){
+
+
+ }
+ else
+ if(d3_button){
+
+
+ }
+ else
+ if(d8_button){
+
+
+ }
+ else
+ if(d9_button){
+
+
+ }
+ else
+ if(d10_button){
+
+
+ }
+ else
+ if(d11_button){
+
- msgRecFlag = false;//clear the message received flag
+ }
+ else
+ if(d12_button){
+
- if(memcmp(rxMsgStore,mc_Tx_Buffer,strlen(rxMsgStore)) == 0){
- led1=1;
- }
- }
- }
-}
\ No newline at end of file
+ }
+ else
+ t1.reset();
+ }
+
+ //mc_debug.printf("cmdCnt = %d", cmdCnt);
+/*
+ switch(cmdCnt){
+ case 0: setRotMotor(CCW, mc_Tx_Buffer);break;//free run the motor CCW
+ case 1: setSpeed(123456, mc_Tx_Buffer);break;//123.456 RPM
+ case 2: setRotMotor(CW, mc_Tx_Buffer);break;//free run the motor CW
+ case 3: setTorque(3456, mc_Tx_Buffer);break;//3.456N.m
+ case 4: setTorque(7000, mc_Tx_Buffer);break;//7N.m
+ case 5: set_mL(CW, 1000, mc_Tx_Buffer);break;//dispence 1000ml running CW
+ case 6: set_mL(CCW, 1000, mc_Tx_Buffer);break;//dispence 1000ml running CW
+ }
+*/
+/*
+ switch(cmdCnt){
+ case READ_DIG_IN: readDigInput(mc_Tx_Buffer);break;
+ case READ_DIG_OUT: readDigOutput(mc_Tx_Buffer);break;
+ case READ_CURRENT: readCurrent(mc_Tx_Buffer);break;
+ case READ_DRV_REG: readDrvReg(mc_Tx_Buffer);break;
+ case READ_DRV_REG_EXT: readDrvRegExt(mc_Tx_Buffer);break;
+ case READ_DRV_TEMP: readTemp(mc_Tx_Buffer);break;
+ case READ_DRV_VOLTAGE: readVoltage(mc_Tx_Buffer);break;
+ case READ_DRV_WORKING_SET: readDrvWorkSet(mc_Tx_Buffer);break;
+ case READ_DRV_WORKING_SET_EXT: readDrvWorkSetExt(mc_Tx_Buffer);break;
+ case READ_ERROR_REG: readErrReg(mc_Tx_Buffer);break;
+ case READ_FB_BOOST_CUR: readFBboostCur(mc_Tx_Buffer);break;
+ case READ_FB_STATUS: readFBstat(mc_Tx_Buffer);break;
+ case READ_FW_VERSION: readFWVwersion(mc_Tx_Buffer);break;
+ case READ_FW_CHECKSUM: readFWChecksum(mc_Tx_Buffer);break;
+ case READ_MASTER_REG: readMastReg(mc_Tx_Buffer);break;
+ case READ_MIN_CURRENT: readMinCur(mc_Tx_Buffer);break;
+ case READ_MAX_CURRENT: readMaxCur(mc_Tx_Buffer);break;
+ case READ_BOOST_CURRENT: readBoostCur(mc_Tx_Buffer);break;
+ case READ_NOM_CURRENT: readNomCur(mc_Tx_Buffer);break;
+ }
+*/
+ /*
+ if(rxMsgPending == false){
+ rxMsgPending = true;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+*/
+
+ //while(getMotMsg(mc_Tx_Buffer, rxMsgStore) == RX_ECHO_FAIL);//message has failed so resent the last message again and loop a nuber of re-tries if required.
+
+
+ if(getMotMsg(mc_Tx_Buffer, rxMsgStore) == REBOOT){
+ mc_usart.printf("\r\n\r\nRebooting the system\r\n\r\n");
+ NVIC_SystemReset();
+ }
+
+
+ }//2
+}//1
\ No newline at end of file
diff -r 333434a8611b -r 0cb065f9d55a main.h --- a/main.h Fri Mar 20 10:54:58 2020 +0000 +++ b/main.h Mon May 18 19:04:41 2020 +0000 @@ -2,6 +2,16 @@ #define MAIN_H #include "mbed.h" +#include <string> +extern RawSerial mc_debug; +extern RawSerial mc_usart;// this will be USART6 for motor control +extern string HARDWARE; +extern string SOFTWARE; +extern string AUTHOR; + +extern uint8_t cmdCnt; +extern DigitalOut led1; +extern bool rxMsgPending; #endif \ No newline at end of file
diff -r 333434a8611b -r 0cb065f9d55a readCommands.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/readCommands.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<string>
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// MOTOR COMMAND READ FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void readDigInput(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read digital inputs
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DIGITAL INPUTS\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readDigOutput(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read digital inputs
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DIGITAL OUTPUTS\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readCurrent(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read the motor current command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ CURRENT ACTUAL\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readDrvReg(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read drive register command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DRIVE REGISTER\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readDrvRegExt(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read drive register extended command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DRIVE REGISTER EXTENDED\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readTemp(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read motor temperature command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DRIVE TEMPERATURE\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readVoltage(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read motor voltage command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DRIVE VOLTAGE\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readDrvWorkSet(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read drive working settings command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DRIVE WORKING SETTINGS\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readDrvWorkSetExt(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ DRIVE WORKING SETTINGS EXTENDED\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readErrReg(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read error register command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ ERROR REGISTER\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readFBboostCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ FEEDBACK BOOST CURRENT\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readFBstat(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ FEEDBACK STATUS\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readFWVwersion(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read the firmware version command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ FIRMWARE VERSION\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+
+}
+
+void readFWChecksum(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read the firmware checksum command
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ FIRMWARE CHECKSUM\r");
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readMastReg(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ MASTER REGISTER\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readMinCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ MIN CURRENT\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readMaxCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ MAX CURRENT\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readBoostCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ BOOST CURRENT\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
+
+void readNomCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+////////////////////////////////////////////////////////////////////////////////
+ if(rxMsgPending == false){
+ sprintf(array,"READ NOMINAL CURRENT\r");//<CR>
+ mc_debug.printf("%s",array);
+ //mc_usart.printf("%s",array);
+ rxMsgPending = true;
+ led1=0;
+
+ if(cmdCnt < COMMANDS_TO_TEST)
+ cmdCnt++;
+ else
+ cmdCnt=0;
+ }
+}
diff -r 333434a8611b -r 0cb065f9d55a readCommands.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/readCommands.h Mon May 18 19:04:41 2020 +0000 @@ -0,0 +1,27 @@ +#ifndef READCOMMANDS_H +#define READCOMMANDS_H + +#include "mbed.h" +#include <string> + +void readDigOutput(char *array); +void readErrReg(char *array); +void readTemp(char *array); +void readVoltage(char *array); +void readDrvReg(char *array); +void readDrvRegExt(char *array); +void readDrvWorkSet(char *array); +void readDrvWorkSetExt(char *array); +void readMastReg(char *array); +void readFWVwersion(char *array); +void readFWChecksum(char *array); +void readFBboostCur(char *array); +void readFBstat(char *array); +void readMinCur(char *array); +void readMaxCur(char *array); +void readBoostCur(char *array); +void readMasterReg(char *array); +void readNomCur(char *array); + + +#endif \ No newline at end of file
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);
+}
+
+
+
+
+
diff -r 333434a8611b -r 0cb065f9d55a writeCommands.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/writeCommands.h Mon May 18 19:04:41 2020 +0000 @@ -0,0 +1,15 @@ +#ifndef WRITECOMMANDS_H +#define WRITECOMMANDS_H + +#include "mbed.h" +#include <string> + + +bool moveMot(uint8_t par1, int32_t par2, char *array); +bool setSpeed(float *n_speed, char *array); +bool stopMot(uint8_t par1, int32_t par2, char *array); +bool setTorque(uint16_t newtonMilliMetres, char *array); +bool setWorkSet(uint16_t par1, char *array); +bool setWorkSetExt(uint16_t par1, char *array); + +#endif \ No newline at end of file