J L
ringBuffer26
Diff: MotorControl.cpp
- Revision:
- 1:0cb065f9d55a
- Parent:
- 0:333434a8611b
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