J L
/
ringBuffer26a
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Embed:
(wiki syntax)
Show/hide line numbers
writeCommands.cpp
00001 #include <stdio.h> 00002 #include "mbed.h" 00003 #include "main.h" 00004 #include "MotorControl.h" 00005 #include "writeCommands.h" 00006 #include<string> 00007 00008 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00009 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00010 // PRODUCT COMMAND FUNCTIONS 00011 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00012 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00013 00014 bool setTorque(uint16_t newtonMilliMetres, char *array){ 00015 //////////////////////////////////////////////////////////////////////////////// 00016 // Set Torque 00017 // 00018 // newtonMilliMetres : (0 - 8500) : 0 - 8.5 N.m 00019 // 00020 // return : invalid param = 1, ok = 0 00021 // 00022 //////////////////////////////////////////////////////////////////////////////// 00023 00024 bool invParm = false; 00025 00026 if(rxMsgPending == false){ 00027 if(newtonMilliMetres <= MAX_TORQUE){ 00028 sprintf(array,"SET FEEDBACK BOOST CURRENT %u\r",newtonMilliMetres); 00029 mc_debug.printf("%s",array); 00030 //mc_usart.printf("%s",array); 00031 00032 #ifdef DEBUG 00033 //mc_debug.printf("Set Torque %u\r\n", newtonMilliMetres); 00034 mc_usart.printf("Set Torque %u\r\n", newtonMilliMetres); 00035 #endif 00036 } 00037 else{ 00038 invParm = true; 00039 #ifdef DEBUG 00040 //mc_debug.printf("set Torque parameter fail\r\n"); 00041 mc_usart.printf("set Torque parameter fail\r\n"); 00042 #endif 00043 } 00044 /* 00045 if(cmdCnt < COMMANDS_TO_TEST) 00046 cmdCnt++; 00047 else 00048 cmdCnt=0; 00049 */ 00050 //rxMsgPending = true; 00051 led1=0; 00052 } 00053 return(invParm); 00054 } 00055 00056 00057 bool set_mL(bool direction, uint32_t mL, char *array){ 00058 //////////////////////////////////////////////////////////////////////////////// 00059 // Set milli litres 00060 // 00061 // direction : CW = 1, CCW = 0 00062 // 00063 // mL : 0 - 2147483648, -2147483648 - 0 : 2.1 billion 00064 // 00065 // return : invalid param = 1, ok = 0 00066 // 00067 //////////////////////////////////////////////////////////////////////////////// 00068 00069 bool invParm = false; 00070 uint32_t steps = 0; 00071 00072 if(rxMsgPending == false){ 00073 steps = ((mL/1000)/(ML_ROTATION/1000))*STEPS_PER_ROTATION; 00074 00075 if(direction == CW) 00076 invParm = moveMot(2, steps, array); 00077 else 00078 if(direction == CCW) 00079 invParm = moveMot(3, steps, array); 00080 00081 #ifdef DEBUG 00082 if(direction == CW){ 00083 //mc_debug.printf("Dispense CW %d steps\r\n", steps); 00084 mc_usart.printf("Dispense CW %d steps\r\n", steps); 00085 } 00086 else 00087 if(direction == CCW){ 00088 //mc_debug.printf("Dispense CCW %d steps\r\n", steps); 00089 mc_usart.printf("Dispense CCW %d steps\r\n", steps); 00090 } 00091 00092 #endif 00093 /* 00094 if(cmdCnt < COMMANDS_TO_TEST) 00095 cmdCnt++; 00096 else 00097 cmdCnt=0; 00098 */ 00099 //rxMsgPending = true; 00100 led1=0; 00101 } 00102 return(invParm); 00103 } 00104 00105 bool setRotMotor(bool direction, char *array){ 00106 //////////////////////////////////////////////////////////////////////////////// 00107 // Rotate motor 00108 // 00109 // direction : CW = 1, CCW = 0 00110 // 00111 // return : invalid param = 1, ok = 0 00112 // 00113 //////////////////////////////////////////////////////////////////////////////// 00114 00115 bool invParm = false; 00116 00117 if(rxMsgPending == false){ 00118 if(direction == CW) 00119 invParm = moveMot(0, 0, array); 00120 else 00121 if(direction == CCW) 00122 invParm = moveMot(1, 0, array); 00123 00124 #ifdef DEBUG 00125 if(direction == CW){ 00126 //mc_debug.printf("MOVEMOT free run CW\r\n"); 00127 mc_usart.printf("MOVEMOT free run CW\r\n"); 00128 } 00129 else 00130 if(direction == CCW){ 00131 //mc_debug.printf("MOVEMOT free run CCW\r\n"); 00132 mc_usart.printf("MOVEMOT free run CCW\r\n"); 00133 } 00134 00135 #endif 00136 /* 00137 if(cmdCnt < COMMANDS_TO_TEST) 00138 cmdCnt++; 00139 else 00140 cmdCnt=0; 00141 */ 00142 //rxMsgPending = true; 00143 led1=0; 00144 } 00145 return(invParm); 00146 } 00147 00148 bool stopMot(uint8_t par1, int32_t par2, char *array){ 00149 //////////////////////////////////////////////////////////////////////////////// 00150 // Stop Motor 00151 // 00152 // par1 = 0 : stop with no ramp | par2 : not used 00153 // par1 = 1 : stop with ramp | par2 : not used 00154 // par1 = 2 : stop with steps | par2 : number of stop steps 00155 // 00156 // return : invalid param = 1 00157 // 00158 //////////////////////////////////////////////////////////////////////////////// 00159 00160 static uint8_t newPar1 = 0xff; 00161 static int32_t newPar2 = 0xff; 00162 bool invParm = false; 00163 00164 //if((par1 != newPar1) || (par2 != newPar2)){ 00165 00166 newPar1 = par1; 00167 newPar2 = par2; 00168 00169 if(((par1 == STOP_NO_RAMP)||(par1 == STOP_WITH_RAMP))&&(par2 == NOT_USED)){ 00170 sprintf(array,"STOPMOT %u %u\r",par1, par2); 00171 mc_debug.printf("%s",array); 00172 //mc_usart.printf("%s",array); 00173 00174 #ifdef DEBUG 00175 //mc_debug.printf("STOPMOT %u %u\r",par1, par2); 00176 mc_usart.printf("STOPMOT %u %u\r",par1, par2); 00177 #endif 00178 } 00179 else 00180 if((par1 == STOP_WITH_STEPS)&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){ 00181 sprintf(array,"STOPMOT %u %u\r",par1, par2); 00182 mc_debug.printf("%s",array); 00183 //mc_usart.printf("%s",array); 00184 00185 #ifdef DEBUG 00186 //mc_debug.printf("STOPMOT %u %u\r",par1, par2); 00187 mc_usart.printf("STOPMOT %u %u\r",par1, par2); 00188 #endif 00189 } 00190 else{ 00191 invParm = true; 00192 #ifdef DEBUG 00193 //mc_debug.printf("STOPMOT parameter fail\r\n"); 00194 mc_usart.printf("STOPMOT parameter fail\r\n"); 00195 #endif 00196 } 00197 //} 00198 led1=0; 00199 00200 return(invParm); 00201 } 00202 00203 bool setSpeed(float *n_speed, char *array){ 00204 //////////////////////////////////////////////////////////////////////////////// 00205 // Set Speed 00206 // 00207 // 1000 = 0.001 RPM 00208 // 3000000 = 125.0 RPM 00209 // 00210 // range 1000 - 3000000 00211 // 00212 // speed :(1 - 125000) : 0.001 - 125.000 RPM 00213 // 00214 // return : invalid param = 1, ok = 0 00215 // 00216 //////////////////////////////////////////////////////////////////////////////// 00217 00218 bool invParm = false; 00219 uint32_t ed_speed;//motor controller speed, 1000 - 3000000 00220 00221 if((*n_speed >= N_MIN_SPEED)&&(*n_speed <= N_MAX_SPEED)){ 00222 00223 ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED); 00224 00225 sprintf(array,"SET PROFILE VELOCITY %u\r",ed_speed); 00226 mc_debug.printf("%s",array); 00227 00228 #ifdef DEBUG 00229 //mc_debug.printf("ED_SetSpeed %u, Neptune_SetSpeed %f\r\n", ed_speed, n_speed); 00230 mc_usart.printf("ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); 00231 #endif 00232 00233 led1=0; 00234 } 00235 else{ 00236 00237 if(*n_speed > N_MAX_SPEED) 00238 *n_speed = N_MAX_SPEED; 00239 else 00240 if(*n_speed < N_MIN_SPEED) 00241 *n_speed = N_MIN_SPEED; 00242 00243 00244 /* 00245 #ifdef DEBUG 00246 ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED); 00247 //mc_debug.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); 00248 mc_usart.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); 00249 #endif 00250 */ 00251 } 00252 return(invParm); 00253 } 00254 00255 bool setWorkSetExt(uint16_t par1, char *array){ 00256 //////////////////////////////////////////////////////////////////////////////// 00257 // Set Working Settings Extended 00258 // 00259 // par1 : bit 5 = disable auto motor enable 00260 // par1 : bit 6 = encoder rotation direction 00261 // par1 : bit 9 = RL motor detection 00262 // par1 : bit 10 = enable I2T protection 00263 // 00264 // return : invalid param = 1, ok = 0 00265 // 00266 //////////////////////////////////////////////////////////////////////////////// 00267 00268 static uint8_t newPar1 = 0xff; 00269 bool invParm = false; 00270 00271 if(rxMsgPending == false){ 00272 if(par1 != newPar1){ 00273 00274 newPar1 = par1; 00275 00276 if(par1 <= 15){ 00277 sprintf(array,"SET DRIVE WORKING SETTINGS EXTENDED %u\r",par1); 00278 mc_debug.printf("%s",array); 00279 //mc_usart.printf("%s",array); 00280 00281 #ifdef DEBUG 00282 //mc_debug.printf("Drive working setting extended %u\r\n", par1); 00283 mc_usart.printf("Drive working setting extended %u\r\n", par1); 00284 #endif 00285 } 00286 else{ 00287 invParm = true; 00288 #ifdef DEBUG 00289 //mc_debug.printf("setWorkSet parameter fail\r\n"); 00290 mc_usart.printf("setWorkSet parameter fail\r\n"); 00291 #endif 00292 } 00293 } 00294 /* 00295 if(cmdCnt < COMMANDS_TO_TEST) 00296 cmdCnt++; 00297 else 00298 cmdCnt=0; 00299 */ 00300 //rxMsgPending = true; 00301 led1=0; 00302 } 00303 return(invParm); 00304 } 00305 00306 bool moveMot(uint8_t par1, int32_t par2, char *array){ 00307 //////////////////////////////////////////////////////////////////////////////// 00308 // Move Motor 00309 // 00310 // par1 = 0 : move free running forward | par2 : not used 00311 // par1 = 1 : move free running backwards | par2 : not used 00312 // par1 = 2 : move step forward | par2 : number of steps 00313 // par1 = 3 : move step backwards | par2 : number of steps 00314 // par1 - 4 : move to target posistion | par2 : posistion to reach 00315 // 00316 // return : invalid param = 1, ok = 0 00317 // 00318 //////////////////////////////////////////////////////////////////////////////// 00319 00320 static uint8_t newPar1 = 0xff; 00321 static int32_t newPar2 = 0xff; 00322 bool invParm = false; 00323 00324 if(rxMsgPending == false){ 00325 if((par1 != newPar1) || (par2 != newPar2)){ 00326 00327 newPar1 = par1; 00328 newPar2 = par2; 00329 00330 if(((par1 >=2)&&(par1 <=4))&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){ 00331 sprintf(array,"MOVEMOT %u %u\r",par1, par2); 00332 mc_debug.printf("%s",array); 00333 //mc_usart.printf("%s",array); 00334 00335 #ifdef DEBUG 00336 //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2); 00337 mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2); 00338 #endif 00339 } 00340 else 00341 if((par1 == 0)||(par1 ==1)){ 00342 sprintf(array,"MOVEMOT %u %u\r",par1, 0); 00343 mc_debug.printf("%s\r",array); 00344 mc_usart.printf("%s",array); 00345 00346 #ifdef DEBUG 00347 //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2); 00348 mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2); 00349 #endif 00350 } 00351 else{ 00352 invParm = true; 00353 #ifdef DEBUG 00354 mc_debug.printf("movMot parameter fail\r\n"); 00355 mc_usart.printf("movMot parameter fail\r\n"); 00356 #endif 00357 } 00358 } 00359 /* 00360 if(cmdCnt < COMMANDS_TO_TEST) 00361 cmdCnt++; 00362 else 00363 cmdCnt=0; 00364 */ 00365 //rxMsgPending = true; 00366 led1=0; 00367 } 00368 return(invParm); 00369 } 00370 00371 00372 00373 00374
Generated on Wed Jul 13 2022 03:47:44 by
1.7.2