J L / Mbed 2 deprecated ringBuffer26

ringBuffer26

Dependencies:   mbed

MotorControl.cpp@1:0cb065f9d55a, 2020-05-18 (annotated)

Committer:
Picmon
Date:
Mon May 18 19:04:41 2020 +0000
Revision:
1:0cb065f9d55a
Parent:
0:333434a8611b 
ring buffer26;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Picmon 1:0cb065f9d55a 1 /*
Picmon 1:0cb065f9d55a 2
Picmon 1:0cb065f9d55a 3 EOL SEQUENCE = NONE ON YAT, PROTOCOL WILL FAIL IF THIS IS NOT DONE
Picmon 0:333434a8611b 4
Picmon 1:0cb065f9d55a 5 */
Picmon 1:0cb065f9d55a 6
Picmon 1:0cb065f9d55a 7
Picmon 1:0cb065f9d55a 8
Picmon 0:333434a8611b 9
Picmon 1:0cb065f9d55a 10 #include <stdio.h>
Picmon 1:0cb065f9d55a 11 #include "mbed.h"
Picmon 1:0cb065f9d55a 12 #include "main.h"
Picmon 1:0cb065f9d55a 13 #include "MotorControl.h"
Picmon 1:0cb065f9d55a 14 #include<string>
Picmon 0:333434a8611b 15
Picmon 1:0cb065f9d55a 16 class motorControl{
Picmon 1:0cb065f9d55a 17
Picmon 1:0cb065f9d55a 18 private:
Picmon 1:0cb065f9d55a 19
Picmon 1:0cb065f9d55a 20 public:
Picmon 1:0cb065f9d55a 21
Picmon 1:0cb065f9d55a 22 protected:
Picmon 0:333434a8611b 23
Picmon 0:333434a8611b 24 };
Picmon 0:333434a8611b 25
Picmon 1:0cb065f9d55a 26 char *ptr;
Picmon 1:0cb065f9d55a 27
Picmon 1:0cb065f9d55a 28 volatile uint8_t count = 0;
Picmon 1:0cb065f9d55a 29
Picmon 1:0cb065f9d55a 30 bool msgValid = false;
Picmon 1:0cb065f9d55a 31 bool msgRdy = false;
Picmon 1:0cb065f9d55a 32
Picmon 1:0cb065f9d55a 33 uint8_t i=0;
Picmon 1:0cb065f9d55a 34
Picmon 1:0cb065f9d55a 35 DigitalIn d3_button(D3_BUTTON);
Picmon 1:0cb065f9d55a 36 DigitalIn d4_button(D4_BUTTON);
Picmon 1:0cb065f9d55a 37 DigitalIn d5_button(D5_BUTTON);
Picmon 1:0cb065f9d55a 38 DigitalIn d6_button(D6_BUTTON);
Picmon 1:0cb065f9d55a 39 DigitalIn d7_button(D7_BUTTON);
Picmon 1:0cb065f9d55a 40 DigitalIn d8_button(D8_BUTTON);
Picmon 1:0cb065f9d55a 41 DigitalIn d9_button(D9_BUTTON);
Picmon 1:0cb065f9d55a 42 DigitalIn d10_button(D10_BUTTON);
Picmon 1:0cb065f9d55a 43 DigitalIn d11_button(D11_BUTTON);
Picmon 1:0cb065f9d55a 44 DigitalIn d12_button(D12_BUTTON);
Picmon 1:0cb065f9d55a 45
Picmon 1:0cb065f9d55a 46 DigitalOut a7_out1(A7_OUT1);//mot enable
Picmon 1:0cb065f9d55a 47 DigitalOut a6_out2(A6_OUT2);
Picmon 1:0cb065f9d55a 48 DigitalOut a5_out3(A5_OUT3);
Picmon 0:333434a8611b 49
Picmon 1:0cb065f9d55a 50 DigitalIn a4_in1(A4_IN1);
Picmon 1:0cb065f9d55a 51 DigitalIn a3_in2(A3_IN2);
Picmon 1:0cb065f9d55a 52 DigitalIn a2_in3(A2_IN3);
Picmon 1:0cb065f9d55a 53
Picmon 1:0cb065f9d55a 54 DigitalOut a1_out(A1_OUT);
Picmon 1:0cb065f9d55a 55 DigitalOut a0_out(A0_OUT);
Picmon 1:0cb065f9d55a 56
Picmon 1:0cb065f9d55a 57
Picmon 1:0cb065f9d55a 58 char rxMsgStore[BUFFER_SIZE];//received message store
Picmon 0:333434a8611b 59 char mc_Tx_Buffer[BUFFER_SIZE];//transmitted message buffer
Picmon 1:0cb065f9d55a 60 char mc_Rx_Buffer[BUFFER_SIZE];//received message buffer
Picmon 1:0cb065f9d55a 61 volatile unsigned char mc_Rx_Rd_Ptr = 0;//circular buffer read pointer
Picmon 1:0cb065f9d55a 62 volatile unsigned char mc_Rx_Wr_Ptr = 0;//circular buffer write pointer
Picmon 1:0cb065f9d55a 63 volatile bool rxMsgValid = false;//message received flag
Picmon 1:0cb065f9d55a 64 volatile bool rxMsgInvalid = false;//message invalid flag
Picmon 0:333434a8611b 65
Picmon 1:0cb065f9d55a 66 char motData[DATA_ITEMS][DATA_BUFFER_SIZE];//data only
Picmon 1:0cb065f9d55a 67
Picmon 1:0cb065f9d55a 68 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 69 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 70 // MOTOR ISR FUNCTIONS
Picmon 1:0cb065f9d55a 71 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 72 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 0:333434a8611b 73
Picmon 0:333434a8611b 74 void motMsg_RX_ISR(void){
Picmon 0:333434a8611b 75
Picmon 1:0cb065f9d55a 76 mc_Rx_Buffer[mc_Rx_Wr_Ptr] = mc_debug.getc();
Picmon 0:333434a8611b 77
Picmon 1:0cb065f9d55a 78 mc_Rx_Wr_Ptr++;
Picmon 1:0cb065f9d55a 79 count++;
Picmon 1:0cb065f9d55a 80
Picmon 1:0cb065f9d55a 81 if(mc_Rx_Wr_Ptr == (BUFFER_SIZE - 1))
Picmon 1:0cb065f9d55a 82 mc_Rx_Wr_Ptr = 0;
Picmon 0:333434a8611b 83
Picmon 0:333434a8611b 84 }
Picmon 0:333434a8611b 85
Picmon 1:0cb065f9d55a 86 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 87 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 88 // MOTOR READ FUNCTIONS
Picmon 1:0cb065f9d55a 89 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 90 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 91
Picmon 1:0cb065f9d55a 92 uint8_t getMotMsg(char *txArray, char *rxArray){
Picmon 1:0cb065f9d55a 93 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 94 // get the motor message
Picmon 1:0cb065f9d55a 95 //
Picmon 1:0cb065f9d55a 96 // funtion receives command messages eched from the motor controller, the
Picmon 1:0cb065f9d55a 97 // message eched is checked and verified, if correct the message is parsed
Picmon 1:0cb065f9d55a 98 // to expose the numerical data, this data is stored in the relevant memory
Picmon 1:0cb065f9d55a 99 // location for later use
Picmon 1:0cb065f9d55a 100 //
Picmon 1:0cb065f9d55a 101 //
Picmon 1:0cb065f9d55a 102 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 103 uint8_t msgStatus = NULL;
Picmon 1:0cb065f9d55a 104 static uint8_t msgReTry = 0;
Picmon 1:0cb065f9d55a 105
Picmon 1:0cb065f9d55a 106 while(mc_Rx_Wr_Ptr != mc_Rx_Rd_Ptr){// if not true we have a message to collect
Picmon 1:0cb065f9d55a 107
Picmon 1:0cb065f9d55a 108 rxArray[i] = mc_Rx_Buffer[mc_Rx_Rd_Ptr];//write the charcter into the array element indexed by i
Picmon 0:333434a8611b 109
Picmon 1:0cb065f9d55a 110 if(rxArray[i] == ACK)//read the character from the array elecment indexed by i and if it is <ACK> set message valid flag (msgValid)
Picmon 1:0cb065f9d55a 111 msgValid = true;
Picmon 1:0cb065f9d55a 112
Picmon 1:0cb065f9d55a 113 #ifdef DEBUG
Picmon 1:0cb065f9d55a 114 //mc_debug.printf("%c", rxArray[i]);//print out chahacters from array indexed by i
Picmon 1:0cb065f9d55a 115 //mc_usart.printf("%c", rxArray[i]);//print out chahacters from array indexed by i
Picmon 1:0cb065f9d55a 116 #endif
Picmon 1:0cb065f9d55a 117
Picmon 1:0cb065f9d55a 118 i++;//increment the array index
Picmon 1:0cb065f9d55a 119
Picmon 1:0cb065f9d55a 120 mc_Rx_Rd_Ptr++;//increment the read pointer
Picmon 1:0cb065f9d55a 121
Picmon 1:0cb065f9d55a 122 count--;
Picmon 1:0cb065f9d55a 123
Picmon 1:0cb065f9d55a 124 if(mc_Rx_Rd_Ptr == (BUFFER_SIZE - 1))
Picmon 1:0cb065f9d55a 125 mc_Rx_Rd_Ptr = 0;
Picmon 1:0cb065f9d55a 126 }
Picmon 1:0cb065f9d55a 127
Picmon 1:0cb065f9d55a 128 if((mc_Rx_Wr_Ptr == mc_Rx_Rd_Ptr)&&(msgValid == true)){//full message received
Picmon 1:0cb065f9d55a 129
Picmon 1:0cb065f9d55a 130 msgValid = false;
Picmon 1:0cb065f9d55a 131
Picmon 1:0cb065f9d55a 132 if(strncmp(txArray, rxArray, strlen(txArray))==0){
Picmon 1:0cb065f9d55a 133 #ifdef DEBUG
Picmon 1:0cb065f9d55a 134 //mc_debug.printf("\r\nEcho compare is Successful, txArray = %s, rxArray = %s\r\n", txArray, rxArray);
Picmon 1:0cb065f9d55a 135 mc_usart.printf("\r\nEcho compare is Successful, txArray = %s, rxArray = %s\r\n", txArray, rxArray);
Picmon 1:0cb065f9d55a 136 #endif
Picmon 1:0cb065f9d55a 137
Picmon 1:0cb065f9d55a 138 msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 139 msgReTry=0;
Picmon 1:0cb065f9d55a 140
Picmon 1:0cb065f9d55a 141 //READ COMMAND TYPE 1 : <COMMAND><CR><VALUE><ACK>
Picmon 1:0cb065f9d55a 142 if(strncmp(rxArray,"READ DIGITAL INPUTS\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 143 led1=1;
Picmon 1:0cb065f9d55a 144 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 145 //msgReTry=0;
Picmon 1:0cb065f9d55a 146 data2mem(READ_DIG_IN, motData, rxArray);
Picmon 1:0cb065f9d55a 147 #ifdef DEBUG
Picmon 1:0cb065f9d55a 148 //mc_debug.printf("\r\nArray Contents %s\r\n\r\n", motData[READ_DIG_IN]);
Picmon 1:0cb065f9d55a 149 mc_usart.printf("\r\nArray Contents %s\r\n\r\n", motData[READ_DIG_IN]);
Picmon 1:0cb065f9d55a 150 #endif
Picmon 1:0cb065f9d55a 151 }
Picmon 1:0cb065f9d55a 152 else
Picmon 1:0cb065f9d55a 153 if(strncmp(rxArray,"READ DIGITAL OUTPUTS\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 154 led1=1;
Picmon 1:0cb065f9d55a 155 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 156 //msgReTry=0;
Picmon 1:0cb065f9d55a 157 data2mem(READ_DIG_OUT, motData, rxArray);
Picmon 1:0cb065f9d55a 158 #ifdef DEBUG
Picmon 1:0cb065f9d55a 159 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DIG_OUT]);
Picmon 1:0cb065f9d55a 160 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DIG_OUT]);
Picmon 1:0cb065f9d55a 161 #endif
Picmon 1:0cb065f9d55a 162 }
Picmon 1:0cb065f9d55a 163 else
Picmon 1:0cb065f9d55a 164 if(strncmp(rxArray,"READ CURRENT ACTUAL\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 165 led1=1;
Picmon 1:0cb065f9d55a 166 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 167 //msgReTry=0;
Picmon 1:0cb065f9d55a 168 data2mem(READ_CURRENT, motData, rxArray);
Picmon 1:0cb065f9d55a 169 #ifdef DEBUG
Picmon 1:0cb065f9d55a 170 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_CURRENT]);
Picmon 1:0cb065f9d55a 171 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_CURRENT]);
Picmon 1:0cb065f9d55a 172 #endif
Picmon 1:0cb065f9d55a 173 }
Picmon 1:0cb065f9d55a 174 else
Picmon 1:0cb065f9d55a 175 if(strncmp(rxArray,"READ DRIVE REGISTER\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 176 led1=1;
Picmon 1:0cb065f9d55a 177 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 178 //msgReTry=0;
Picmon 1:0cb065f9d55a 179 data2mem(READ_DRV_REG, motData, rxArray);
Picmon 1:0cb065f9d55a 180 #ifdef DEBUG
Picmon 1:0cb065f9d55a 181 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG]);
Picmon 1:0cb065f9d55a 182 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG]);
Picmon 1:0cb065f9d55a 183 #endif
Picmon 1:0cb065f9d55a 184 }
Picmon 1:0cb065f9d55a 185 else
Picmon 1:0cb065f9d55a 186 if(strncmp(rxArray,"READ DRIVE REGISTER EXTENDED\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 187 led1=1;
Picmon 1:0cb065f9d55a 188 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 189 //msgReTry=0;
Picmon 1:0cb065f9d55a 190 data2mem(READ_DRV_REG_EXT, motData, rxArray);
Picmon 1:0cb065f9d55a 191 #ifdef DEBUG
Picmon 1:0cb065f9d55a 192 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG_EXT]);
Picmon 1:0cb065f9d55a 193 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG_EXT]);
Picmon 1:0cb065f9d55a 194 #endif
Picmon 1:0cb065f9d55a 195 }
Picmon 1:0cb065f9d55a 196 else
Picmon 1:0cb065f9d55a 197 if(strncmp(rxArray,"READ DRIVE TEMPERATURE\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 198 led1=1;
Picmon 1:0cb065f9d55a 199 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 200 //msgReTry=0;
Picmon 1:0cb065f9d55a 201 data2mem(READ_DRV_TEMP, motData, rxArray);
Picmon 1:0cb065f9d55a 202 #ifdef DEBUG
Picmon 1:0cb065f9d55a 203 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_TEMP]);
Picmon 1:0cb065f9d55a 204 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_TEMP]);
Picmon 1:0cb065f9d55a 205 #endif
Picmon 1:0cb065f9d55a 206 }
Picmon 1:0cb065f9d55a 207 else
Picmon 1:0cb065f9d55a 208 if(strncmp(rxArray,"READ DRIVE VOLTAGE\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 209 led1=1;
Picmon 1:0cb065f9d55a 210 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 211 //msgReTry=0;
Picmon 1:0cb065f9d55a 212 data2mem(READ_DRV_VOLTAGE, motData, rxArray);
Picmon 1:0cb065f9d55a 213 #ifdef DEBUG
Picmon 1:0cb065f9d55a 214 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_VOLTAGE]);
Picmon 1:0cb065f9d55a 215 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_VOLTAGE]);
Picmon 1:0cb065f9d55a 216 #endif
Picmon 1:0cb065f9d55a 217 }
Picmon 1:0cb065f9d55a 218 else
Picmon 1:0cb065f9d55a 219 if(strncmp(rxArray,"READ DRIVE WORKING SETTINGS\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 220 led1=1;
Picmon 1:0cb065f9d55a 221 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 222 //msgReTry=0;
Picmon 1:0cb065f9d55a 223 data2mem(READ_DRV_WORKING_SET, motData, rxArray);
Picmon 1:0cb065f9d55a 224 #ifdef DEBUG
Picmon 1:0cb065f9d55a 225 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET]);
Picmon 1:0cb065f9d55a 226 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET]);
Picmon 1:0cb065f9d55a 227 #endif
Picmon 1:0cb065f9d55a 228 }
Picmon 1:0cb065f9d55a 229 else
Picmon 1:0cb065f9d55a 230 if(strncmp(rxArray,"READ DRIVE WORKING SETTINGS EXTENDED\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 231 led1=1;
Picmon 1:0cb065f9d55a 232 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 233 //msgReTry=0;
Picmon 1:0cb065f9d55a 234 data2mem(READ_DRV_WORKING_SET_EXT, motData, rxArray);
Picmon 1:0cb065f9d55a 235 #ifdef DEBUG
Picmon 1:0cb065f9d55a 236 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET_EXT]);
Picmon 1:0cb065f9d55a 237 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET_EXT]);
Picmon 1:0cb065f9d55a 238 #endif
Picmon 1:0cb065f9d55a 239 }
Picmon 1:0cb065f9d55a 240 else
Picmon 1:0cb065f9d55a 241 if(strncmp(rxArray,"READ ERROR REGISTER\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 242 led1=1;
Picmon 1:0cb065f9d55a 243 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 244 //msgReTry=0;
Picmon 1:0cb065f9d55a 245 data2mem(READ_ERROR_REG, motData, rxArray);
Picmon 1:0cb065f9d55a 246 #ifdef DEBUG
Picmon 1:0cb065f9d55a 247 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_ERROR_REG]);
Picmon 1:0cb065f9d55a 248 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_ERROR_REG]);
Picmon 1:0cb065f9d55a 249 #endif
Picmon 1:0cb065f9d55a 250 }
Picmon 1:0cb065f9d55a 251 else
Picmon 1:0cb065f9d55a 252 if(strncmp(rxArray,"READ FEEDBACK BOOST CURRENT\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 253 led1=1;
Picmon 1:0cb065f9d55a 254 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 255 //msgReTry=0;
Picmon 1:0cb065f9d55a 256 data2mem(READ_FB_BOOST_CUR, motData, rxArray);
Picmon 1:0cb065f9d55a 257 #ifdef DEBUG
Picmon 1:0cb065f9d55a 258 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FB_BOOST_CUR]);
Picmon 1:0cb065f9d55a 259 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FB_BOOST_CUR]);
Picmon 1:0cb065f9d55a 260 #endif
Picmon 1:0cb065f9d55a 261 }
Picmon 1:0cb065f9d55a 262 else
Picmon 1:0cb065f9d55a 263 if(strncmp(rxArray,"READ FEEDBACK STATUS\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 264 led1=1;
Picmon 1:0cb065f9d55a 265 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 266 //msgReTry=0;
Picmon 1:0cb065f9d55a 267 data2mem(READ_FB_STATUS, motData, rxArray);
Picmon 1:0cb065f9d55a 268 #ifdef DEBUG
Picmon 1:0cb065f9d55a 269 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FB_STATUS]);
Picmon 1:0cb065f9d55a 270 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FB_STATUS]);
Picmon 1:0cb065f9d55a 271 #endif
Picmon 1:0cb065f9d55a 272 }
Picmon 1:0cb065f9d55a 273 else
Picmon 1:0cb065f9d55a 274 if(strncmp(rxArray,"READ FIRMWARE VERSION\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 275 led1=1;
Picmon 1:0cb065f9d55a 276 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 277 //msgReTry=0;
Picmon 1:0cb065f9d55a 278 data2mem(READ_FW_VERSION, motData, rxArray);
Picmon 1:0cb065f9d55a 279 #ifdef DEBUG
Picmon 1:0cb065f9d55a 280 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FW_VERSION]);
Picmon 1:0cb065f9d55a 281 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FW_VERSION]);
Picmon 1:0cb065f9d55a 282 #endif
Picmon 1:0cb065f9d55a 283 }
Picmon 1:0cb065f9d55a 284 else
Picmon 1:0cb065f9d55a 285 if(strncmp(rxArray,"READ FIRMWARE CHECKSUM\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 286 led1=1;
Picmon 1:0cb065f9d55a 287 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 288 //msgReTry=0;
Picmon 1:0cb065f9d55a 289 data2mem(READ_FW_CHECKSUM, motData, rxArray);
Picmon 1:0cb065f9d55a 290 #ifdef DEBUG
Picmon 1:0cb065f9d55a 291 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FW_CHECKSUM]);
Picmon 1:0cb065f9d55a 292 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FW_CHECKSUM]);
Picmon 1:0cb065f9d55a 293 #endif
Picmon 1:0cb065f9d55a 294 }
Picmon 1:0cb065f9d55a 295 else
Picmon 1:0cb065f9d55a 296 if(strncmp(rxArray,"READ MASTER REGISTER\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 297 led1=1;
Picmon 1:0cb065f9d55a 298 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 299 //msgReTry=0;
Picmon 1:0cb065f9d55a 300 data2mem(READ_MASTER_REG, motData, rxArray);
Picmon 1:0cb065f9d55a 301 #ifdef DEBUG
Picmon 1:0cb065f9d55a 302 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MASTER_REG]);
Picmon 1:0cb065f9d55a 303 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MASTER_REG]);
Picmon 1:0cb065f9d55a 304 #endif
Picmon 1:0cb065f9d55a 305 }
Picmon 1:0cb065f9d55a 306 else
Picmon 1:0cb065f9d55a 307 if(strncmp(rxArray,"READ MIN CURRENT\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 308 led1=1;
Picmon 1:0cb065f9d55a 309 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 310 //msgReTry=0;
Picmon 1:0cb065f9d55a 311 data2mem(READ_MIN_CURRENT, motData, rxArray);
Picmon 1:0cb065f9d55a 312 #ifdef DEBUG
Picmon 1:0cb065f9d55a 313 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MIN_CURRENT]);
Picmon 1:0cb065f9d55a 314 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MIN_CURRENT]);
Picmon 1:0cb065f9d55a 315 #endif
Picmon 1:0cb065f9d55a 316 }
Picmon 1:0cb065f9d55a 317 else
Picmon 1:0cb065f9d55a 318 if(strncmp(rxArray,"READ MAX CURRENT\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 319 led1=1;
Picmon 1:0cb065f9d55a 320 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 321 //msgReTry=0;
Picmon 1:0cb065f9d55a 322 data2mem(READ_MAX_CURRENT, motData, rxArray);
Picmon 1:0cb065f9d55a 323 #ifdef DEBUG
Picmon 1:0cb065f9d55a 324 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MAX_CURRENT]);
Picmon 1:0cb065f9d55a 325 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MAX_CURRENT]);
Picmon 1:0cb065f9d55a 326 #endif
Picmon 1:0cb065f9d55a 327 }
Picmon 1:0cb065f9d55a 328 else
Picmon 1:0cb065f9d55a 329 if(strncmp(rxArray,"READ BOOST CURRENT\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 330 led1=1;
Picmon 1:0cb065f9d55a 331 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 332 //msgReTry=0;
Picmon 1:0cb065f9d55a 333 data2mem(READ_BOOST_CURRENT, motData, rxArray);
Picmon 1:0cb065f9d55a 334 #ifdef DEBUG
Picmon 1:0cb065f9d55a 335 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_BOOST_CURRENT]);
Picmon 1:0cb065f9d55a 336 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_BOOST_CURRENT]);
Picmon 1:0cb065f9d55a 337 #endif
Picmon 1:0cb065f9d55a 338 }
Picmon 1:0cb065f9d55a 339 else
Picmon 1:0cb065f9d55a 340 if(strncmp(rxArray,"READ NOMINAL CURRENT\r", strlen(txArray))==0){
Picmon 1:0cb065f9d55a 341 led1=1;
Picmon 1:0cb065f9d55a 342 //msgStatus = RX_ECHO_VALID;
Picmon 1:0cb065f9d55a 343 //msgReTry=0;
Picmon 1:0cb065f9d55a 344 data2mem(READ_NOM_CURRENT, motData, rxArray);
Picmon 1:0cb065f9d55a 345 #ifdef DEBUG
Picmon 1:0cb065f9d55a 346 //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_NOM_CURRENT]);
Picmon 1:0cb065f9d55a 347 mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_NOM_CURRENT]);
Picmon 1:0cb065f9d55a 348 #endif
Picmon 1:0cb065f9d55a 349 }
Picmon 1:0cb065f9d55a 350 }
Picmon 1:0cb065f9d55a 351 else{
Picmon 0:333434a8611b 352
Picmon 1:0cb065f9d55a 353 msgStatus = RX_ECHO_FAIL;
Picmon 1:0cb065f9d55a 354
Picmon 1:0cb065f9d55a 355 if(msgReTry < MSG_RETRY_COUNT){//limit message transmit re-tries
Picmon 1:0cb065f9d55a 356 mc_debug.printf("%s",txArray);//transmit the last message again
Picmon 1:0cb065f9d55a 357 msgReTry++;//increment the re-try counter
Picmon 0:333434a8611b 358
Picmon 1:0cb065f9d55a 359 #ifdef DEBUG
Picmon 1:0cb065f9d55a 360 //mc_usart.printf("\r\nEcho compare FAILED, txArray = %s, rxArray = %s\r\n", txArray, rxArray);
Picmon 1:0cb065f9d55a 361 mc_usart.printf("\r\nTransmit re-try = %d, txArray = %s, rxArray = %s, txArray Length = %d\r\n", msgReTry , txArray, rxArray, strlen(txArray));
Picmon 1:0cb065f9d55a 362 #endif
Picmon 1:0cb065f9d55a 363 }
Picmon 1:0cb065f9d55a 364 else{
Picmon 1:0cb065f9d55a 365 #ifdef DEBUG
Picmon 1:0cb065f9d55a 366 //mc_debug.printf("\r\nEcho compare has FAILED, txArray = %s, rxArray = %s, txArray Length = %d\r\n", txArray, rxArray, strlen(txArray));
Picmon 1:0cb065f9d55a 367 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));
Picmon 1:0cb065f9d55a 368 #endif
Picmon 1:0cb065f9d55a 369 msgStatus = REBOOT;
Picmon 1:0cb065f9d55a 370 msgReTry=0;
Picmon 1:0cb065f9d55a 371 }
Picmon 1:0cb065f9d55a 372 }
Picmon 1:0cb065f9d55a 373
Picmon 1:0cb065f9d55a 374 i=0;//reset the array indexer for next message
Picmon 1:0cb065f9d55a 375 memset(rxArray, NULL, BUFFER_SIZE);//clear the entire buffer for next message
Picmon 0:333434a8611b 376 }
Picmon 1:0cb065f9d55a 377 return(msgStatus);
Picmon 0:333434a8611b 378 }
Picmon 0:333434a8611b 379
Picmon 0:333434a8611b 380
Picmon 1:0cb065f9d55a 381 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 382 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 383 // mc_debug FUNCTIONS
Picmon 1:0cb065f9d55a 384 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 385 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 386
Picmon 1:0cb065f9d55a 387
Picmon 1:0cb065f9d55a 388 void data2mem(uint8_t dataLoc, char destMemArray[][DATA_BUFFER_SIZE], char sourceMemArray[BUFFER_SIZE]){
Picmon 1:0cb065f9d55a 389 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 390 // data to memory for command type 1
Picmon 1:0cb065f9d55a 391 //
Picmon 1:0cb065f9d55a 392 //
Picmon 1:0cb065f9d55a 393 //
Picmon 1:0cb065f9d55a 394 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 395
Picmon 1:0cb065f9d55a 396 //parse the string to isolate the VALUE
Picmon 1:0cb065f9d55a 397 //<COMMAND><CR><VALUE><ACK>
Picmon 1:0cb065f9d55a 398
Picmon 1:0cb065f9d55a 399 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
Picmon 0:333434a8611b 400
Picmon 1:0cb065f9d55a 401 *ptr = '\0';//overwrite <ACK> character with null terminator character, this will serve as end of string terminataor now
Picmon 1:0cb065f9d55a 402
Picmon 1:0cb065f9d55a 403 ptr = strchr(sourceMemArray, '\r');//now set the pointer first <CR> address in the string
Picmon 1:0cb065f9d55a 404
Picmon 1:0cb065f9d55a 405 *ptr++;//point to first character of VALUE (MSB) in the string, <CR> + 1 = <VALUE>
Picmon 1:0cb065f9d55a 406
Picmon 1:0cb065f9d55a 407 strcpy(destMemArray[dataLoc], ptr);//copy the <VALUE> to the destination memory location
Picmon 1:0cb065f9d55a 408 }
Picmon 1:0cb065f9d55a 409
Picmon 1:0cb065f9d55a 410 void Init(void){
Picmon 1:0cb065f9d55a 411
Picmon 1:0cb065f9d55a 412 mc_debug.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
Picmon 1:0cb065f9d55a 413 //mc_usart.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
Picmon 1:0cb065f9d55a 414 led1=0;
Picmon 1:0cb065f9d55a 415
Picmon 1:0cb065f9d55a 416
Picmon 1:0cb065f9d55a 417 mc_usart.printf("\r\n ED Motor Controler API");
Picmon 1:0cb065f9d55a 418 mc_usart.printf("\r\n\r\n HARDWARE \t\t= %s", HARDWARE.c_str());
Picmon 1:0cb065f9d55a 419 mc_usart.printf("\r\n\r\n SOFTWARE \t\t= %s",SOFTWARE.c_str());
Picmon 1:0cb065f9d55a 420 mc_usart.printf("\r\n\r\n AUTHOR \t\t= %s",AUTHOR.c_str());
Picmon 1:0cb065f9d55a 421 mc_usart.printf("\r\n\r\n DATE \t\t\t= %s",__DATE__);
Picmon 1:0cb065f9d55a 422 mc_usart.printf("\r\n\r\n TIME \t\t\t= %s\r\n\r\n",__TIME__);
Picmon 0:333434a8611b 423
Picmon 1:0cb065f9d55a 424
Picmon 1:0cb065f9d55a 425 /*
Picmon 1:0cb065f9d55a 426 d3_button.mode(PullUp);
Picmon 1:0cb065f9d55a 427 d4_button.mode(PullUp);
Picmon 1:0cb065f9d55a 428 d5_button.mode(PullUp);
Picmon 1:0cb065f9d55a 429 d6_button.mode(PullUp);
Picmon 1:0cb065f9d55a 430 d7_button.mode(PullUp);
Picmon 1:0cb065f9d55a 431 d8_button.mode(PullUp);
Picmon 1:0cb065f9d55a 432 d9_button.mode(PullUp);
Picmon 1:0cb065f9d55a 433 d10_button.mode(PullUp);
Picmon 1:0cb065f9d55a 434 d11_button.mode(PullUp);
Picmon 1:0cb065f9d55a 435 d12_button.mode(PullUp);
Picmon 1:0cb065f9d55a 436 */
Picmon 1:0cb065f9d55a 437
Picmon 1:0cb065f9d55a 438 a7_out1 = 0;//motor enable
Picmon 1:0cb065f9d55a 439 a6_out2 = 0;
Picmon 1:0cb065f9d55a 440 a5_out3 = 0;
Picmon 1:0cb065f9d55a 441
Picmon 0:333434a8611b 442 }
Picmon 1:0cb065f9d55a 443
Picmon 1:0cb065f9d55a 444 long mapI(long x, long in_min, long in_max, long out_min, long out_max){
Picmon 1:0cb065f9d55a 445 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
Picmon 1:0cb065f9d55a 446 }
Picmon 1:0cb065f9d55a 447
Picmon 1:0cb065f9d55a 448 float mapF(float in, float inMin, float inMax, float outMin, float outMax){
Picmon 1:0cb065f9d55a 449 // check it's within the range
Picmon 1:0cb065f9d55a 450 if (inMin<inMax) {
Picmon 1:0cb065f9d55a 451 if (in <= inMin)
Picmon 1:0cb065f9d55a 452 return outMin;
Picmon 1:0cb065f9d55a 453 if (in >= inMax)
Picmon 1:0cb065f9d55a 454 return outMax;
Picmon 1:0cb065f9d55a 455 } else { // cope with input range being backwards.
Picmon 1:0cb065f9d55a 456 if (in >= inMin)
Picmon 1:0cb065f9d55a 457 return outMin;
Picmon 1:0cb065f9d55a 458 if (in <= inMax)
Picmon 1:0cb065f9d55a 459 return outMax;
Picmon 1:0cb065f9d55a 460 }
Picmon 1:0cb065f9d55a 461 // calculate how far into the range we are
Picmon 1:0cb065f9d55a 462 float scale = (in-inMin)/(inMax-inMin);
Picmon 1:0cb065f9d55a 463 // calculate the output.
Picmon 1:0cb065f9d55a 464 return outMin + scale*(outMax-outMin);
Picmon 1:0cb065f9d55a 465 }