J L / Mbed 2 deprecated ringBuffer26

ringBuffer26

Dependencies:   mbed

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Picmon 1:0cb065f9d55a 1 #include <stdio.h>
Picmon 1:0cb065f9d55a 2 #include "mbed.h"
Picmon 1:0cb065f9d55a 3 #include "main.h"
Picmon 1:0cb065f9d55a 4 #include "MotorControl.h"
Picmon 1:0cb065f9d55a 5 #include "writeCommands.h"
Picmon 1:0cb065f9d55a 6 #include<string>
Picmon 1:0cb065f9d55a 7
Picmon 1:0cb065f9d55a 8 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 9 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 10 // PRODUCT COMMAND FUNCTIONS
Picmon 1:0cb065f9d55a 11 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 12 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 13
Picmon 1:0cb065f9d55a 14 bool setTorque(uint16_t newtonMilliMetres, char *array){
Picmon 1:0cb065f9d55a 15 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 16 // Set Torque
Picmon 1:0cb065f9d55a 17 //
Picmon 1:0cb065f9d55a 18 // newtonMilliMetres : (0 - 8500) : 0 - 8.5 N.m
Picmon 1:0cb065f9d55a 19 //
Picmon 1:0cb065f9d55a 20 // return : invalid param = 1, ok = 0
Picmon 1:0cb065f9d55a 21 //
Picmon 1:0cb065f9d55a 22 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 23
Picmon 1:0cb065f9d55a 24 bool invParm = false;
Picmon 1:0cb065f9d55a 25
Picmon 1:0cb065f9d55a 26 if(rxMsgPending == false){
Picmon 1:0cb065f9d55a 27 if(newtonMilliMetres <= MAX_TORQUE){
Picmon 1:0cb065f9d55a 28 sprintf(array,"SET FEEDBACK BOOST CURRENT %u\r",newtonMilliMetres);
Picmon 1:0cb065f9d55a 29 mc_debug.printf("%s",array);
Picmon 1:0cb065f9d55a 30 //mc_usart.printf("%s",array);
Picmon 1:0cb065f9d55a 31
Picmon 1:0cb065f9d55a 32 #ifdef DEBUG
Picmon 1:0cb065f9d55a 33 //mc_debug.printf("Set Torque %u\r\n", newtonMilliMetres);
Picmon 1:0cb065f9d55a 34 mc_usart.printf("Set Torque %u\r\n", newtonMilliMetres);
Picmon 1:0cb065f9d55a 35 #endif
Picmon 1:0cb065f9d55a 36 }
Picmon 1:0cb065f9d55a 37 else{
Picmon 1:0cb065f9d55a 38 invParm = true;
Picmon 1:0cb065f9d55a 39 #ifdef DEBUG
Picmon 1:0cb065f9d55a 40 //mc_debug.printf("set Torque parameter fail\r\n");
Picmon 1:0cb065f9d55a 41 mc_usart.printf("set Torque parameter fail\r\n");
Picmon 1:0cb065f9d55a 42 #endif
Picmon 1:0cb065f9d55a 43 }
Picmon 1:0cb065f9d55a 44 /*
Picmon 1:0cb065f9d55a 45 if(cmdCnt < COMMANDS_TO_TEST)
Picmon 1:0cb065f9d55a 46 cmdCnt++;
Picmon 1:0cb065f9d55a 47 else
Picmon 1:0cb065f9d55a 48 cmdCnt=0;
Picmon 1:0cb065f9d55a 49 */
Picmon 1:0cb065f9d55a 50 //rxMsgPending = true;
Picmon 1:0cb065f9d55a 51 led1=0;
Picmon 1:0cb065f9d55a 52 }
Picmon 1:0cb065f9d55a 53 return(invParm);
Picmon 1:0cb065f9d55a 54 }
Picmon 1:0cb065f9d55a 55
Picmon 1:0cb065f9d55a 56
Picmon 1:0cb065f9d55a 57 bool set_mL(bool direction, uint32_t mL, char *array){
Picmon 1:0cb065f9d55a 58 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 59 // Set milli litres
Picmon 1:0cb065f9d55a 60 //
Picmon 1:0cb065f9d55a 61 // direction : CW = 1, CCW = 0
Picmon 1:0cb065f9d55a 62 //
Picmon 1:0cb065f9d55a 63 // mL : 0 - 2147483648, -2147483648 - 0 : 2.1 billion
Picmon 1:0cb065f9d55a 64 //
Picmon 1:0cb065f9d55a 65 // return : invalid param = 1, ok = 0
Picmon 1:0cb065f9d55a 66 //
Picmon 1:0cb065f9d55a 67 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 68
Picmon 1:0cb065f9d55a 69 bool invParm = false;
Picmon 1:0cb065f9d55a 70 uint32_t steps = 0;
Picmon 1:0cb065f9d55a 71
Picmon 1:0cb065f9d55a 72 if(rxMsgPending == false){
Picmon 1:0cb065f9d55a 73 steps = ((mL/1000)/(ML_ROTATION/1000))*STEPS_PER_ROTATION;
Picmon 1:0cb065f9d55a 74
Picmon 1:0cb065f9d55a 75 if(direction == CW)
Picmon 1:0cb065f9d55a 76 invParm = moveMot(2, steps, array);
Picmon 1:0cb065f9d55a 77 else
Picmon 1:0cb065f9d55a 78 if(direction == CCW)
Picmon 1:0cb065f9d55a 79 invParm = moveMot(3, steps, array);
Picmon 1:0cb065f9d55a 80
Picmon 1:0cb065f9d55a 81 #ifdef DEBUG
Picmon 1:0cb065f9d55a 82 if(direction == CW){
Picmon 1:0cb065f9d55a 83 //mc_debug.printf("Dispense CW %d steps\r\n", steps);
Picmon 1:0cb065f9d55a 84 mc_usart.printf("Dispense CW %d steps\r\n", steps);
Picmon 1:0cb065f9d55a 85 }
Picmon 1:0cb065f9d55a 86 else
Picmon 1:0cb065f9d55a 87 if(direction == CCW){
Picmon 1:0cb065f9d55a 88 //mc_debug.printf("Dispense CCW %d steps\r\n", steps);
Picmon 1:0cb065f9d55a 89 mc_usart.printf("Dispense CCW %d steps\r\n", steps);
Picmon 1:0cb065f9d55a 90 }
Picmon 1:0cb065f9d55a 91
Picmon 1:0cb065f9d55a 92 #endif
Picmon 1:0cb065f9d55a 93 /*
Picmon 1:0cb065f9d55a 94 if(cmdCnt < COMMANDS_TO_TEST)
Picmon 1:0cb065f9d55a 95 cmdCnt++;
Picmon 1:0cb065f9d55a 96 else
Picmon 1:0cb065f9d55a 97 cmdCnt=0;
Picmon 1:0cb065f9d55a 98 */
Picmon 1:0cb065f9d55a 99 //rxMsgPending = true;
Picmon 1:0cb065f9d55a 100 led1=0;
Picmon 1:0cb065f9d55a 101 }
Picmon 1:0cb065f9d55a 102 return(invParm);
Picmon 1:0cb065f9d55a 103 }
Picmon 1:0cb065f9d55a 104
Picmon 1:0cb065f9d55a 105 bool setRotMotor(bool direction, char *array){
Picmon 1:0cb065f9d55a 106 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 107 // Rotate motor
Picmon 1:0cb065f9d55a 108 //
Picmon 1:0cb065f9d55a 109 // direction : CW = 1, CCW = 0
Picmon 1:0cb065f9d55a 110 //
Picmon 1:0cb065f9d55a 111 // return : invalid param = 1, ok = 0
Picmon 1:0cb065f9d55a 112 //
Picmon 1:0cb065f9d55a 113 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 114
Picmon 1:0cb065f9d55a 115 bool invParm = false;
Picmon 1:0cb065f9d55a 116
Picmon 1:0cb065f9d55a 117 if(rxMsgPending == false){
Picmon 1:0cb065f9d55a 118 if(direction == CW)
Picmon 1:0cb065f9d55a 119 invParm = moveMot(0, 0, array);
Picmon 1:0cb065f9d55a 120 else
Picmon 1:0cb065f9d55a 121 if(direction == CCW)
Picmon 1:0cb065f9d55a 122 invParm = moveMot(1, 0, array);
Picmon 1:0cb065f9d55a 123
Picmon 1:0cb065f9d55a 124 #ifdef DEBUG
Picmon 1:0cb065f9d55a 125 if(direction == CW){
Picmon 1:0cb065f9d55a 126 //mc_debug.printf("MOVEMOT free run CW\r\n");
Picmon 1:0cb065f9d55a 127 mc_usart.printf("MOVEMOT free run CW\r\n");
Picmon 1:0cb065f9d55a 128 }
Picmon 1:0cb065f9d55a 129 else
Picmon 1:0cb065f9d55a 130 if(direction == CCW){
Picmon 1:0cb065f9d55a 131 //mc_debug.printf("MOVEMOT free run CCW\r\n");
Picmon 1:0cb065f9d55a 132 mc_usart.printf("MOVEMOT free run CCW\r\n");
Picmon 1:0cb065f9d55a 133 }
Picmon 1:0cb065f9d55a 134
Picmon 1:0cb065f9d55a 135 #endif
Picmon 1:0cb065f9d55a 136 /*
Picmon 1:0cb065f9d55a 137 if(cmdCnt < COMMANDS_TO_TEST)
Picmon 1:0cb065f9d55a 138 cmdCnt++;
Picmon 1:0cb065f9d55a 139 else
Picmon 1:0cb065f9d55a 140 cmdCnt=0;
Picmon 1:0cb065f9d55a 141 */
Picmon 1:0cb065f9d55a 142 //rxMsgPending = true;
Picmon 1:0cb065f9d55a 143 led1=0;
Picmon 1:0cb065f9d55a 144 }
Picmon 1:0cb065f9d55a 145 return(invParm);
Picmon 1:0cb065f9d55a 146 }
Picmon 1:0cb065f9d55a 147
Picmon 1:0cb065f9d55a 148 bool stopMot(uint8_t par1, int32_t par2, char *array){
Picmon 1:0cb065f9d55a 149 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 150 // Stop Motor
Picmon 1:0cb065f9d55a 151 //
Picmon 1:0cb065f9d55a 152 // par1 = 0 : stop with no ramp | par2 : not used
Picmon 1:0cb065f9d55a 153 // par1 = 1 : stop with ramp | par2 : not used
Picmon 1:0cb065f9d55a 154 // par1 = 2 : stop with steps | par2 : number of stop steps
Picmon 1:0cb065f9d55a 155 //
Picmon 1:0cb065f9d55a 156 // return : invalid param = 1
Picmon 1:0cb065f9d55a 157 //
Picmon 1:0cb065f9d55a 158 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 159
Picmon 1:0cb065f9d55a 160 static uint8_t newPar1 = 0xff;
Picmon 1:0cb065f9d55a 161 static int32_t newPar2 = 0xff;
Picmon 1:0cb065f9d55a 162 bool invParm = false;
Picmon 1:0cb065f9d55a 163
Picmon 1:0cb065f9d55a 164 //if((par1 != newPar1) || (par2 != newPar2)){
Picmon 1:0cb065f9d55a 165
Picmon 1:0cb065f9d55a 166 newPar1 = par1;
Picmon 1:0cb065f9d55a 167 newPar2 = par2;
Picmon 1:0cb065f9d55a 168
Picmon 1:0cb065f9d55a 169 if(((par1 == STOP_NO_RAMP)||(par1 == STOP_WITH_RAMP))&&(par2 == NOT_USED)){
Picmon 1:0cb065f9d55a 170 sprintf(array,"STOPMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 171 mc_debug.printf("%s",array);
Picmon 1:0cb065f9d55a 172 //mc_usart.printf("%s",array);
Picmon 1:0cb065f9d55a 173
Picmon 1:0cb065f9d55a 174 #ifdef DEBUG
Picmon 1:0cb065f9d55a 175 //mc_debug.printf("STOPMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 176 mc_usart.printf("STOPMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 177 #endif
Picmon 1:0cb065f9d55a 178 }
Picmon 1:0cb065f9d55a 179 else
Picmon 1:0cb065f9d55a 180 if((par1 == STOP_WITH_STEPS)&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){
Picmon 1:0cb065f9d55a 181 sprintf(array,"STOPMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 182 mc_debug.printf("%s",array);
Picmon 1:0cb065f9d55a 183 //mc_usart.printf("%s",array);
Picmon 1:0cb065f9d55a 184
Picmon 1:0cb065f9d55a 185 #ifdef DEBUG
Picmon 1:0cb065f9d55a 186 //mc_debug.printf("STOPMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 187 mc_usart.printf("STOPMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 188 #endif
Picmon 1:0cb065f9d55a 189 }
Picmon 1:0cb065f9d55a 190 else{
Picmon 1:0cb065f9d55a 191 invParm = true;
Picmon 1:0cb065f9d55a 192 #ifdef DEBUG
Picmon 1:0cb065f9d55a 193 //mc_debug.printf("STOPMOT parameter fail\r\n");
Picmon 1:0cb065f9d55a 194 mc_usart.printf("STOPMOT parameter fail\r\n");
Picmon 1:0cb065f9d55a 195 #endif
Picmon 1:0cb065f9d55a 196 }
Picmon 1:0cb065f9d55a 197 //}
Picmon 1:0cb065f9d55a 198 led1=0;
Picmon 1:0cb065f9d55a 199
Picmon 1:0cb065f9d55a 200 return(invParm);
Picmon 1:0cb065f9d55a 201 }
Picmon 1:0cb065f9d55a 202
Picmon 1:0cb065f9d55a 203 bool setSpeed(float *n_speed, char *array){
Picmon 1:0cb065f9d55a 204 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 205 // Set Speed
Picmon 1:0cb065f9d55a 206 //
Picmon 1:0cb065f9d55a 207 // 1000 = 0.001 RPM
Picmon 1:0cb065f9d55a 208 // 3000000 = 125.0 RPM
Picmon 1:0cb065f9d55a 209 //
Picmon 1:0cb065f9d55a 210 // range 1000 - 3000000
Picmon 1:0cb065f9d55a 211 //
Picmon 1:0cb065f9d55a 212 // speed :(1 - 125000) : 0.001 - 125.000 RPM
Picmon 1:0cb065f9d55a 213 //
Picmon 1:0cb065f9d55a 214 // return : invalid param = 1, ok = 0
Picmon 1:0cb065f9d55a 215 //
Picmon 1:0cb065f9d55a 216 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 217
Picmon 1:0cb065f9d55a 218 bool invParm = false;
Picmon 1:0cb065f9d55a 219 uint32_t ed_speed;//motor controller speed, 1000 - 3000000
Picmon 1:0cb065f9d55a 220
Picmon 1:0cb065f9d55a 221 if((*n_speed >= N_MIN_SPEED)&&(*n_speed <= N_MAX_SPEED)){
Picmon 1:0cb065f9d55a 222
Picmon 1:0cb065f9d55a 223 ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED);
Picmon 1:0cb065f9d55a 224
Picmon 1:0cb065f9d55a 225 sprintf(array,"SET PROFILE VELOCITY %u\r",ed_speed);
Picmon 1:0cb065f9d55a 226 mc_debug.printf("%s",array);
Picmon 1:0cb065f9d55a 227
Picmon 1:0cb065f9d55a 228 #ifdef DEBUG
Picmon 1:0cb065f9d55a 229 //mc_debug.printf("ED_SetSpeed %u, Neptune_SetSpeed %f\r\n", ed_speed, n_speed);
Picmon 1:0cb065f9d55a 230 mc_usart.printf("ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed);
Picmon 1:0cb065f9d55a 231 #endif
Picmon 1:0cb065f9d55a 232
Picmon 1:0cb065f9d55a 233 led1=0;
Picmon 1:0cb065f9d55a 234 }
Picmon 1:0cb065f9d55a 235 else{
Picmon 1:0cb065f9d55a 236
Picmon 1:0cb065f9d55a 237 if(*n_speed > N_MAX_SPEED)
Picmon 1:0cb065f9d55a 238 *n_speed = N_MAX_SPEED;
Picmon 1:0cb065f9d55a 239 else
Picmon 1:0cb065f9d55a 240 if(*n_speed < N_MIN_SPEED)
Picmon 1:0cb065f9d55a 241 *n_speed = N_MIN_SPEED;
Picmon 1:0cb065f9d55a 242
Picmon 1:0cb065f9d55a 243
Picmon 1:0cb065f9d55a 244 /*
Picmon 1:0cb065f9d55a 245 #ifdef DEBUG
Picmon 1:0cb065f9d55a 246 ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED);
Picmon 1:0cb065f9d55a 247 //mc_debug.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed);
Picmon 1:0cb065f9d55a 248 mc_usart.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed);
Picmon 1:0cb065f9d55a 249 #endif
Picmon 1:0cb065f9d55a 250 */
Picmon 1:0cb065f9d55a 251 }
Picmon 1:0cb065f9d55a 252 return(invParm);
Picmon 1:0cb065f9d55a 253 }
Picmon 1:0cb065f9d55a 254
Picmon 1:0cb065f9d55a 255 bool setWorkSetExt(uint16_t par1, char *array){
Picmon 1:0cb065f9d55a 256 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 257 // Set Working Settings Extended
Picmon 1:0cb065f9d55a 258 //
Picmon 1:0cb065f9d55a 259 // par1 : bit 5 = disable auto motor enable
Picmon 1:0cb065f9d55a 260 // par1 : bit 6 = encoder rotation direction
Picmon 1:0cb065f9d55a 261 // par1 : bit 9 = RL motor detection
Picmon 1:0cb065f9d55a 262 // par1 : bit 10 = enable I2T protection
Picmon 1:0cb065f9d55a 263 //
Picmon 1:0cb065f9d55a 264 // return : invalid param = 1, ok = 0
Picmon 1:0cb065f9d55a 265 //
Picmon 1:0cb065f9d55a 266 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 267
Picmon 1:0cb065f9d55a 268 static uint8_t newPar1 = 0xff;
Picmon 1:0cb065f9d55a 269 bool invParm = false;
Picmon 1:0cb065f9d55a 270
Picmon 1:0cb065f9d55a 271 if(rxMsgPending == false){
Picmon 1:0cb065f9d55a 272 if(par1 != newPar1){
Picmon 1:0cb065f9d55a 273
Picmon 1:0cb065f9d55a 274 newPar1 = par1;
Picmon 1:0cb065f9d55a 275
Picmon 1:0cb065f9d55a 276 if(par1 <= 15){
Picmon 1:0cb065f9d55a 277 sprintf(array,"SET DRIVE WORKING SETTINGS EXTENDED %u\r",par1);
Picmon 1:0cb065f9d55a 278 mc_debug.printf("%s",array);
Picmon 1:0cb065f9d55a 279 //mc_usart.printf("%s",array);
Picmon 1:0cb065f9d55a 280
Picmon 1:0cb065f9d55a 281 #ifdef DEBUG
Picmon 1:0cb065f9d55a 282 //mc_debug.printf("Drive working setting extended %u\r\n", par1);
Picmon 1:0cb065f9d55a 283 mc_usart.printf("Drive working setting extended %u\r\n", par1);
Picmon 1:0cb065f9d55a 284 #endif
Picmon 1:0cb065f9d55a 285 }
Picmon 1:0cb065f9d55a 286 else{
Picmon 1:0cb065f9d55a 287 invParm = true;
Picmon 1:0cb065f9d55a 288 #ifdef DEBUG
Picmon 1:0cb065f9d55a 289 //mc_debug.printf("setWorkSet parameter fail\r\n");
Picmon 1:0cb065f9d55a 290 mc_usart.printf("setWorkSet parameter fail\r\n");
Picmon 1:0cb065f9d55a 291 #endif
Picmon 1:0cb065f9d55a 292 }
Picmon 1:0cb065f9d55a 293 }
Picmon 1:0cb065f9d55a 294 /*
Picmon 1:0cb065f9d55a 295 if(cmdCnt < COMMANDS_TO_TEST)
Picmon 1:0cb065f9d55a 296 cmdCnt++;
Picmon 1:0cb065f9d55a 297 else
Picmon 1:0cb065f9d55a 298 cmdCnt=0;
Picmon 1:0cb065f9d55a 299 */
Picmon 1:0cb065f9d55a 300 //rxMsgPending = true;
Picmon 1:0cb065f9d55a 301 led1=0;
Picmon 1:0cb065f9d55a 302 }
Picmon 1:0cb065f9d55a 303 return(invParm);
Picmon 1:0cb065f9d55a 304 }
Picmon 1:0cb065f9d55a 305
Picmon 1:0cb065f9d55a 306 bool moveMot(uint8_t par1, int32_t par2, char *array){
Picmon 1:0cb065f9d55a 307 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 308 // Move Motor
Picmon 1:0cb065f9d55a 309 //
Picmon 1:0cb065f9d55a 310 // par1 = 0 : move free running forward | par2 : not used
Picmon 1:0cb065f9d55a 311 // par1 = 1 : move free running backwards | par2 : not used
Picmon 1:0cb065f9d55a 312 // par1 = 2 : move step forward | par2 : number of steps
Picmon 1:0cb065f9d55a 313 // par1 = 3 : move step backwards | par2 : number of steps
Picmon 1:0cb065f9d55a 314 // par1 - 4 : move to target posistion | par2 : posistion to reach
Picmon 1:0cb065f9d55a 315 //
Picmon 1:0cb065f9d55a 316 // return : invalid param = 1, ok = 0
Picmon 1:0cb065f9d55a 317 //
Picmon 1:0cb065f9d55a 318 ////////////////////////////////////////////////////////////////////////////////
Picmon 1:0cb065f9d55a 319
Picmon 1:0cb065f9d55a 320 static uint8_t newPar1 = 0xff;
Picmon 1:0cb065f9d55a 321 static int32_t newPar2 = 0xff;
Picmon 1:0cb065f9d55a 322 bool invParm = false;
Picmon 1:0cb065f9d55a 323
Picmon 1:0cb065f9d55a 324 if(rxMsgPending == false){
Picmon 1:0cb065f9d55a 325 if((par1 != newPar1) || (par2 != newPar2)){
Picmon 1:0cb065f9d55a 326
Picmon 1:0cb065f9d55a 327 newPar1 = par1;
Picmon 1:0cb065f9d55a 328 newPar2 = par2;
Picmon 1:0cb065f9d55a 329
Picmon 1:0cb065f9d55a 330 if(((par1 >=2)&&(par1 <=4))&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){
Picmon 1:0cb065f9d55a 331 sprintf(array,"MOVEMOT %u %u\r",par1, par2);
Picmon 1:0cb065f9d55a 332 mc_debug.printf("%s",array);
Picmon 1:0cb065f9d55a 333 //mc_usart.printf("%s",array);
Picmon 1:0cb065f9d55a 334
Picmon 1:0cb065f9d55a 335 #ifdef DEBUG
Picmon 1:0cb065f9d55a 336 //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2);
Picmon 1:0cb065f9d55a 337 mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2);
Picmon 1:0cb065f9d55a 338 #endif
Picmon 1:0cb065f9d55a 339 }
Picmon 1:0cb065f9d55a 340 else
Picmon 1:0cb065f9d55a 341 if((par1 == 0)||(par1 ==1)){
Picmon 1:0cb065f9d55a 342 sprintf(array,"MOVEMOT %u %u\r",par1, 0);
Picmon 1:0cb065f9d55a 343 mc_debug.printf("%s\r",array);
Picmon 1:0cb065f9d55a 344 mc_usart.printf("%s",array);
Picmon 1:0cb065f9d55a 345
Picmon 1:0cb065f9d55a 346 #ifdef DEBUG
Picmon 1:0cb065f9d55a 347 //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2);
Picmon 1:0cb065f9d55a 348 mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2);
Picmon 1:0cb065f9d55a 349 #endif
Picmon 1:0cb065f9d55a 350 }
Picmon 1:0cb065f9d55a 351 else{
Picmon 1:0cb065f9d55a 352 invParm = true;
Picmon 1:0cb065f9d55a 353 #ifdef DEBUG
Picmon 1:0cb065f9d55a 354 mc_debug.printf("movMot parameter fail\r\n");
Picmon 1:0cb065f9d55a 355 mc_usart.printf("movMot parameter fail\r\n");
Picmon 1:0cb065f9d55a 356 #endif
Picmon 1:0cb065f9d55a 357 }
Picmon 1:0cb065f9d55a 358 }
Picmon 1:0cb065f9d55a 359 /*
Picmon 1:0cb065f9d55a 360 if(cmdCnt < COMMANDS_TO_TEST)
Picmon 1:0cb065f9d55a 361 cmdCnt++;
Picmon 1:0cb065f9d55a 362 else
Picmon 1:0cb065f9d55a 363 cmdCnt=0;
Picmon 1:0cb065f9d55a 364 */
Picmon 1:0cb065f9d55a 365 //rxMsgPending = true;
Picmon 1:0cb065f9d55a 366 led1=0;
Picmon 1:0cb065f9d55a 367 }
Picmon 1:0cb065f9d55a 368 return(invParm);
Picmon 1:0cb065f9d55a 369 }
Picmon 1:0cb065f9d55a 370
Picmon 1:0cb065f9d55a 371
Picmon 1:0cb065f9d55a 372
Picmon 1:0cb065f9d55a 373
Picmon 1:0cb065f9d55a 374