jack cassa / Mbed OS Nucleo_rtos_basic_v2_public

Test

Dependencies:   X_NUCLEO_IHM01A1

main.cpp@3:24ab1c8fefef, 2019-12-29 (annotated)

Committer:
jackcassa1967
Date:
Sun Dec 29 18:36:23 2019 +0000
Revision:
3:24ab1c8fefef
Parent:
2:35f13b7f3659 
Start

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jackcassa1967 3:24ab1c8fefef 1 /**
jackcassa1967 3:24ab1c8fefef 2 ******************************************************************************
jackcassa1967 3:24ab1c8fefef 3 * @file main.cpp
jackcassa1967 3:24ab1c8fefef 4 * @author Davide Aliprandi, STMicroelectronics
jackcassa1967 3:24ab1c8fefef 5 * @version V1.0.0
jackcassa1967 3:24ab1c8fefef 6 * @date October 14th, 2015
jackcassa1967 3:24ab1c8fefef 7 * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
jackcassa1967 3:24ab1c8fefef 8 * Motor Control Expansion Board: control of 2 motors.
jackcassa1967 3:24ab1c8fefef 9 ******************************************************************************
jackcassa1967 3:24ab1c8fefef 10 * @attention
jackcassa1967 3:24ab1c8fefef 11 *
jackcassa1967 3:24ab1c8fefef 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
jackcassa1967 3:24ab1c8fefef 13 *
jackcassa1967 3:24ab1c8fefef 14 * Redistribution and use in source and binary forms, with or without modification,
jackcassa1967 3:24ab1c8fefef 15 * are permitted provided that the following conditions are met:
jackcassa1967 3:24ab1c8fefef 16 * 1. Redistributions of source code must retain the above copyright notice,
jackcassa1967 3:24ab1c8fefef 17 * this list of conditions and the following disclaimer.
jackcassa1967 3:24ab1c8fefef 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
jackcassa1967 3:24ab1c8fefef 19 * this list of conditions and the following disclaimer in the documentation
jackcassa1967 3:24ab1c8fefef 20 * and/or other materials provided with the distribution.
jackcassa1967 3:24ab1c8fefef 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
jackcassa1967 3:24ab1c8fefef 22 * may be used to endorse or promote products derived from this software
jackcassa1967 3:24ab1c8fefef 23 * without specific prior written permission.
jackcassa1967 3:24ab1c8fefef 24 *
jackcassa1967 3:24ab1c8fefef 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
jackcassa1967 3:24ab1c8fefef 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
jackcassa1967 3:24ab1c8fefef 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
jackcassa1967 3:24ab1c8fefef 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
jackcassa1967 3:24ab1c8fefef 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
jackcassa1967 3:24ab1c8fefef 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
jackcassa1967 3:24ab1c8fefef 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
jackcassa1967 3:24ab1c8fefef 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
jackcassa1967 3:24ab1c8fefef 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
jackcassa1967 3:24ab1c8fefef 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
jackcassa1967 3:24ab1c8fefef 35 *
jackcassa1967 3:24ab1c8fefef 36 ******************************************************************************
jackcassa1967 3:24ab1c8fefef 37 */
jackcassa1967 3:24ab1c8fefef 38
jackcassa1967 3:24ab1c8fefef 39
jackcassa1967 3:24ab1c8fefef 40 /* Includes ------------------------------------------------------------------*/
jackcassa1967 3:24ab1c8fefef 41
jackcassa1967 3:24ab1c8fefef 42 /* mbed specific header files. */
bcostm 0:5701b41769fd 43 #include "mbed.h"
bcostm 0:5701b41769fd 44
jackcassa1967 3:24ab1c8fefef 45 /* Helper header files. */
jackcassa1967 3:24ab1c8fefef 46 #include "DevSPI.h"
jackcassa1967 3:24ab1c8fefef 47
jackcassa1967 3:24ab1c8fefef 48 /* Component specific header files. */
jackcassa1967 3:24ab1c8fefef 49 #include "L6474.h"
jackcassa1967 3:24ab1c8fefef 50 #include "L6474_def.h"
jackcassa1967 3:24ab1c8fefef 51
jackcassa1967 3:24ab1c8fefef 52 //#include "steppermotor.h"
jackcassa1967 3:24ab1c8fefef 53
jackcassa1967 3:24ab1c8fefef 54
jackcassa1967 3:24ab1c8fefef 55 /* Definitions ---------------------------------------------------------------*/
jackcassa1967 3:24ab1c8fefef 56
jackcassa1967 3:24ab1c8fefef 57 /* Number of steps. */
jackcassa1967 3:24ab1c8fefef 58 #define STEPS 3200
jackcassa1967 3:24ab1c8fefef 59
jackcassa1967 3:24ab1c8fefef 60 /* Delay in milliseconds. */
jackcassa1967 3:24ab1c8fefef 61 #define DELAY_1 2000
jackcassa1967 3:24ab1c8fefef 62 #define DELAY_2 6000
jackcassa1967 3:24ab1c8fefef 63 #define DELAY_3 8000
jackcassa1967 3:24ab1c8fefef 64
jackcassa1967 3:24ab1c8fefef 65 /* Speed in pps (Pulses Per Second).
jackcassa1967 3:24ab1c8fefef 66 In Full Step mode: 1 pps = 1 step/s).
jackcassa1967 3:24ab1c8fefef 67 In 1/N Step Mode: N pps = 1 step/s). */
jackcassa1967 3:24ab1c8fefef 68 #define SPEED_1 2400
jackcassa1967 3:24ab1c8fefef 69 #define SPEED_2 1200
jackcassa1967 3:24ab1c8fefef 70
jackcassa1967 3:24ab1c8fefef 71
jackcassa1967 3:24ab1c8fefef 72 #include "Tokenizer.h"
jackcassa1967 3:24ab1c8fefef 73
jackcassa1967 3:24ab1c8fefef 74 /* Variables -----------------------------------------------------------------*/
jackcassa1967 3:24ab1c8fefef 75
jackcassa1967 3:24ab1c8fefef 76 /* Motor Control Component. */
jackcassa1967 3:24ab1c8fefef 77 L6474 *motor1;
jackcassa1967 3:24ab1c8fefef 78 L6474 *motor2;
jackcassa1967 3:24ab1c8fefef 79 L6474 *motor3;
jackcassa1967 3:24ab1c8fefef 80
jackcassa1967 3:24ab1c8fefef 81 int step_mode=0;
jackcassa1967 3:24ab1c8fefef 82
jackcassa1967 3:24ab1c8fefef 83
jackcassa1967 3:24ab1c8fefef 84 #define M3
jackcassa1967 3:24ab1c8fefef 85 #define M2
jackcassa1967 3:24ab1c8fefef 86
jackcassa1967 3:24ab1c8fefef 87
jackcassa1967 3:24ab1c8fefef 88 long serial_count=0;
jackcassa1967 3:24ab1c8fefef 89
jackcassa1967 3:24ab1c8fefef 90 InterruptIn InterA4(A4);
jackcassa1967 3:24ab1c8fefef 91 InterruptIn InterA3(A3);
jackcassa1967 3:24ab1c8fefef 92
jackcassa1967 3:24ab1c8fefef 93 DigitalIn PinA4 (A4);
jackcassa1967 3:24ab1c8fefef 94 DigitalIn PinA3 (A3);
jackcassa1967 3:24ab1c8fefef 95
jackcassa1967 3:24ab1c8fefef 96 DigitalOut POWER(A5);
jackcassa1967 3:24ab1c8fefef 97
jackcassa1967 3:24ab1c8fefef 98 DigitalOut myled(LED1);
jackcassa1967 3:24ab1c8fefef 99
jackcassa1967 3:24ab1c8fefef 100
jackcassa1967 3:24ab1c8fefef 101
jackcassa1967 3:24ab1c8fefef 102
jackcassa1967 3:24ab1c8fefef 103 Serial Console(USBTX, USBRX, 115200);
jackcassa1967 3:24ab1c8fefef 104
jackcassa1967 3:24ab1c8fefef 105
jackcassa1967 3:24ab1c8fefef 106
jackcassa1967 3:24ab1c8fefef 107
jackcassa1967 3:24ab1c8fefef 108 void alarm_pinA4_on(){
jackcassa1967 3:24ab1c8fefef 109 printf("ON A4\n");
jackcassa1967 3:24ab1c8fefef 110 myled=1;
jackcassa1967 3:24ab1c8fefef 111 }
jackcassa1967 3:24ab1c8fefef 112
jackcassa1967 3:24ab1c8fefef 113 void alarm_pinA4_off(){
jackcassa1967 3:24ab1c8fefef 114 printf("OFF A4\n");
jackcassa1967 3:24ab1c8fefef 115 myled=0;
jackcassa1967 3:24ab1c8fefef 116 }
jackcassa1967 3:24ab1c8fefef 117
jackcassa1967 3:24ab1c8fefef 118 void alarm_pinA3_on(){
jackcassa1967 3:24ab1c8fefef 119 printf("ON A3\n");
jackcassa1967 3:24ab1c8fefef 120 }
jackcassa1967 3:24ab1c8fefef 121
jackcassa1967 3:24ab1c8fefef 122 void alarm_pinA3_off(){
jackcassa1967 3:24ab1c8fefef 123 printf("OFF A3\n");
jackcassa1967 3:24ab1c8fefef 124 }
jackcassa1967 3:24ab1c8fefef 125
jackcassa1967 3:24ab1c8fefef 126
jackcassa1967 3:24ab1c8fefef 127
jackcassa1967 3:24ab1c8fefef 128
jackcassa1967 3:24ab1c8fefef 129 PlatformMutex stdio_mutex;
jackcassa1967 3:24ab1c8fefef 130
jackcassa1967 3:24ab1c8fefef 131 void sendString(char * line){
jackcassa1967 3:24ab1c8fefef 132
jackcassa1967 3:24ab1c8fefef 133 stdio_mutex.lock();
jackcassa1967 3:24ab1c8fefef 134 printf("%s\n",line);
jackcassa1967 3:24ab1c8fefef 135 stdio_mutex.unlock();
jackcassa1967 3:24ab1c8fefef 136
jackcassa1967 3:24ab1c8fefef 137 }
jackcassa1967 3:24ab1c8fefef 138
jackcassa1967 3:24ab1c8fefef 139
jackcassa1967 3:24ab1c8fefef 140 void SendStatus(){
jackcassa1967 3:24ab1c8fefef 141 unsigned int statusRegister = motor2->get_status();
jackcassa1967 3:24ab1c8fefef 142 char buffer[100] ;
jackcassa1967 3:24ab1c8fefef 143 sprintf(buffer,"%s %d\n","STATUS",statusRegister);
jackcassa1967 3:24ab1c8fefef 144 sendString(buffer);
bcostm 0:5701b41769fd 145 }
bcostm 0:5701b41769fd 146
jackcassa1967 3:24ab1c8fefef 147
jackcassa1967 3:24ab1c8fefef 148
jackcassa1967 3:24ab1c8fefef 149
bcostm 2:35f13b7f3659 150
jackcassa1967 3:24ab1c8fefef 151 int RangeX=0;
jackcassa1967 3:24ab1c8fefef 152 char bufferCommand[100] ;
jackcassa1967 3:24ab1c8fefef 153
jackcassa1967 3:24ab1c8fefef 154 char * test;
bcostm 0:5701b41769fd 155
jackcassa1967 3:24ab1c8fefef 156 void processLine(string line){
jackcassa1967 3:24ab1c8fefef 157
jackcassa1967 3:24ab1c8fefef 158 test=(char *)line.c_str();
jackcassa1967 3:24ab1c8fefef 159
jackcassa1967 3:24ab1c8fefef 160 Tokenizer parser(line," ,");
jackcassa1967 3:24ab1c8fefef 161
jackcassa1967 3:24ab1c8fefef 162 //sring sId=parser.next();
jackcassa1967 3:24ab1c8fefef 163
jackcassa1967 3:24ab1c8fefef 164 string command = parser.next();
jackcassa1967 3:24ab1c8fefef 165
jackcassa1967 3:24ab1c8fefef 166 if (command=="POWER"){
jackcassa1967 3:24ab1c8fefef 167
jackcassa1967 3:24ab1c8fefef 168 string SW = parser.next();
jackcassa1967 3:24ab1c8fefef 169 int iSW=atoi(SW.c_str());
jackcassa1967 3:24ab1c8fefef 170 if (iSW==0){
jackcassa1967 3:24ab1c8fefef 171 POWER=1;
jackcassa1967 3:24ab1c8fefef 172
jackcassa1967 3:24ab1c8fefef 173 }
jackcassa1967 3:24ab1c8fefef 174 else{
jackcassa1967 3:24ab1c8fefef 175 POWER=0;
jackcassa1967 3:24ab1c8fefef 176 wait_ms(5000);
jackcassa1967 3:24ab1c8fefef 177 motor2->enable();
jackcassa1967 3:24ab1c8fefef 178 motor3->enable();
jackcassa1967 3:24ab1c8fefef 179 }
jackcassa1967 3:24ab1c8fefef 180
jackcassa1967 3:24ab1c8fefef 181 sprintf(bufferCommand,"%s %d","POWER",iSW);
jackcassa1967 3:24ab1c8fefef 182 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 183
jackcassa1967 3:24ab1c8fefef 184 }
jackcassa1967 3:24ab1c8fefef 185
jackcassa1967 3:24ab1c8fefef 186 if (command=="INC"){
jackcassa1967 3:24ab1c8fefef 187
jackcassa1967 3:24ab1c8fefef 188 string s_idxMotor = parser.next();
jackcassa1967 3:24ab1c8fefef 189 string s_step = parser.next();
jackcassa1967 3:24ab1c8fefef 190 int i_idxMotor=atoi(s_idxMotor.c_str());
jackcassa1967 3:24ab1c8fefef 191 int i_step=atoi(s_step.c_str());
jackcassa1967 3:24ab1c8fefef 192 L6474 *motor;
jackcassa1967 3:24ab1c8fefef 193 switch (i_idxMotor)
jackcassa1967 3:24ab1c8fefef 194 {
jackcassa1967 3:24ab1c8fefef 195 case 0:
jackcassa1967 3:24ab1c8fefef 196 motor=motor1;
jackcassa1967 3:24ab1c8fefef 197 break;
jackcassa1967 3:24ab1c8fefef 198 case 1:
jackcassa1967 3:24ab1c8fefef 199 motor=motor2;
jackcassa1967 3:24ab1c8fefef 200 break;
jackcassa1967 3:24ab1c8fefef 201 case 2:
jackcassa1967 3:24ab1c8fefef 202 motor=motor3;
jackcassa1967 3:24ab1c8fefef 203 break;
jackcassa1967 3:24ab1c8fefef 204 }
jackcassa1967 3:24ab1c8fefef 205 int CurrX=motor->get_position();
jackcassa1967 3:24ab1c8fefef 206 motor->set_direction(FORWARD);
jackcassa1967 3:24ab1c8fefef 207 for (int i=0;i<i_step;i++){
jackcassa1967 3:24ab1c8fefef 208 if (PinA4==0) {
jackcassa1967 3:24ab1c8fefef 209 motor->Step();
jackcassa1967 3:24ab1c8fefef 210 wait_us(1000);
jackcassa1967 3:24ab1c8fefef 211 }
jackcassa1967 3:24ab1c8fefef 212 }
jackcassa1967 3:24ab1c8fefef 213 int actualX=motor->get_position();
jackcassa1967 3:24ab1c8fefef 214 int CurrEle= 0;
jackcassa1967 3:24ab1c8fefef 215 sprintf(bufferCommand,"%s %d,%d\n","POSX",actualX,CurrEle);
jackcassa1967 3:24ab1c8fefef 216 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 217
jackcassa1967 3:24ab1c8fefef 218
jackcassa1967 3:24ab1c8fefef 219 }
jackcassa1967 3:24ab1c8fefef 220
jackcassa1967 3:24ab1c8fefef 221 if (command=="DEC"){
jackcassa1967 3:24ab1c8fefef 222
jackcassa1967 3:24ab1c8fefef 223 string s_idxMotor = parser.next();
jackcassa1967 3:24ab1c8fefef 224 string s_step = parser.next();
jackcassa1967 3:24ab1c8fefef 225 int i_idxMotor=atoi(s_idxMotor.c_str());
jackcassa1967 3:24ab1c8fefef 226 int i_step=atoi(s_step.c_str());
jackcassa1967 3:24ab1c8fefef 227 L6474 *motor;
jackcassa1967 3:24ab1c8fefef 228 switch (i_idxMotor)
jackcassa1967 3:24ab1c8fefef 229 {
jackcassa1967 3:24ab1c8fefef 230 case 0:
jackcassa1967 3:24ab1c8fefef 231 motor=motor1;
jackcassa1967 3:24ab1c8fefef 232 break;
jackcassa1967 3:24ab1c8fefef 233 case 1:
jackcassa1967 3:24ab1c8fefef 234 motor=motor2;
jackcassa1967 3:24ab1c8fefef 235 break;
jackcassa1967 3:24ab1c8fefef 236 case 2:
jackcassa1967 3:24ab1c8fefef 237 motor=motor3;
jackcassa1967 3:24ab1c8fefef 238 break;
jackcassa1967 3:24ab1c8fefef 239 }
jackcassa1967 3:24ab1c8fefef 240 int CurrX=motor->get_position();
jackcassa1967 3:24ab1c8fefef 241 //if (PinA4==0)
jackcassa1967 3:24ab1c8fefef 242 {
jackcassa1967 3:24ab1c8fefef 243 motor->set_direction(BACKWARD);
jackcassa1967 3:24ab1c8fefef 244 for (int i=0;i<i_step;i++){
jackcassa1967 3:24ab1c8fefef 245 motor->Step();
jackcassa1967 3:24ab1c8fefef 246 wait_us(1000);
jackcassa1967 3:24ab1c8fefef 247 }
bcostm 0:5701b41769fd 248 }
jackcassa1967 3:24ab1c8fefef 249
jackcassa1967 3:24ab1c8fefef 250 int actualX=motor->get_position();
jackcassa1967 3:24ab1c8fefef 251 int CurrEle= 0;
jackcassa1967 3:24ab1c8fefef 252 sprintf(bufferCommand,"%s %d,%d\n","POSX",actualX,CurrEle);
jackcassa1967 3:24ab1c8fefef 253 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 254 }
jackcassa1967 3:24ab1c8fefef 255
jackcassa1967 3:24ab1c8fefef 256 if (command=="HOME"){
jackcassa1967 3:24ab1c8fefef 257
jackcassa1967 3:24ab1c8fefef 258 string s_idxMotor = parser.next();
jackcassa1967 3:24ab1c8fefef 259 int i_idxMotor=atoi(s_idxMotor.c_str());
jackcassa1967 3:24ab1c8fefef 260 L6474 *motor;
jackcassa1967 3:24ab1c8fefef 261 switch (i_idxMotor)
jackcassa1967 3:24ab1c8fefef 262 {
jackcassa1967 3:24ab1c8fefef 263 case 0:
jackcassa1967 3:24ab1c8fefef 264 motor=motor1;
jackcassa1967 3:24ab1c8fefef 265 break;
jackcassa1967 3:24ab1c8fefef 266 case 1:
jackcassa1967 3:24ab1c8fefef 267 motor=motor2;
jackcassa1967 3:24ab1c8fefef 268 break;
jackcassa1967 3:24ab1c8fefef 269 case 2:
jackcassa1967 3:24ab1c8fefef 270 motor=motor3;
jackcassa1967 3:24ab1c8fefef 271 break;
jackcassa1967 3:24ab1c8fefef 272 }
jackcassa1967 3:24ab1c8fefef 273 int CurrX=motor->get_position();
jackcassa1967 3:24ab1c8fefef 274 motor->set_direction(FORWARD);
jackcassa1967 3:24ab1c8fefef 275 while (true)
jackcassa1967 3:24ab1c8fefef 276 {
jackcassa1967 3:24ab1c8fefef 277 if (PinA4==0)
jackcassa1967 3:24ab1c8fefef 278 {
jackcassa1967 3:24ab1c8fefef 279 motor->Step();
jackcassa1967 3:24ab1c8fefef 280 wait_us(1000);
jackcassa1967 3:24ab1c8fefef 281 }
jackcassa1967 3:24ab1c8fefef 282 else
jackcassa1967 3:24ab1c8fefef 283 break;
jackcassa1967 3:24ab1c8fefef 284
jackcassa1967 3:24ab1c8fefef 285 }
jackcassa1967 3:24ab1c8fefef 286 int actualX=motor->get_position();
jackcassa1967 3:24ab1c8fefef 287 int CurrEle= 0;
jackcassa1967 3:24ab1c8fefef 288 sprintf(bufferCommand,"%s %d,%d\n","POSX",actualX,CurrEle);
jackcassa1967 3:24ab1c8fefef 289 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 290
jackcassa1967 3:24ab1c8fefef 291
jackcassa1967 3:24ab1c8fefef 292 }
jackcassa1967 3:24ab1c8fefef 293
jackcassa1967 3:24ab1c8fefef 294
jackcassa1967 3:24ab1c8fefef 295
jackcassa1967 3:24ab1c8fefef 296 if (command=="STEP_MODE"){
jackcassa1967 3:24ab1c8fefef 297
jackcassa1967 3:24ab1c8fefef 298 string s_step = parser.next();
jackcassa1967 3:24ab1c8fefef 299
jackcassa1967 3:24ab1c8fefef 300 StepperMotor::step_mode_t mode=StepperMotor::STEP_MODE_UNKNOWN ;
jackcassa1967 3:24ab1c8fefef 301
jackcassa1967 3:24ab1c8fefef 302 if (s_step.compare("Full")==0){
jackcassa1967 3:24ab1c8fefef 303 mode=StepperMotor::STEP_MODE_FULL;
jackcassa1967 3:24ab1c8fefef 304 }
jackcassa1967 3:24ab1c8fefef 305 else if (s_step.compare("Half")==0){
jackcassa1967 3:24ab1c8fefef 306 mode=StepperMotor::STEP_MODE_HALF;
jackcassa1967 3:24ab1c8fefef 307 }
jackcassa1967 3:24ab1c8fefef 308 else if (s_step.compare("1/4")==0){
jackcassa1967 3:24ab1c8fefef 309 mode=StepperMotor::STEP_MODE_1_4;
jackcassa1967 3:24ab1c8fefef 310 }
jackcassa1967 3:24ab1c8fefef 311 else if (s_step.compare("1/8")==0){
jackcassa1967 3:24ab1c8fefef 312 mode=StepperMotor::STEP_MODE_1_8;
jackcassa1967 3:24ab1c8fefef 313 }
jackcassa1967 3:24ab1c8fefef 314 else if (s_step.compare("1/16")==0){
jackcassa1967 3:24ab1c8fefef 315 mode=StepperMotor::STEP_MODE_1_16;
jackcassa1967 3:24ab1c8fefef 316 }
jackcassa1967 3:24ab1c8fefef 317 if (mode!=StepperMotor::STEP_MODE_UNKNOWN)
jackcassa1967 3:24ab1c8fefef 318 {
jackcassa1967 3:24ab1c8fefef 319 motor1->set_step_mode(mode);
jackcassa1967 3:24ab1c8fefef 320 motor2->set_step_mode(mode);
jackcassa1967 3:24ab1c8fefef 321 motor3->set_step_mode(mode);
jackcassa1967 3:24ab1c8fefef 322 motor2->enable();
jackcassa1967 3:24ab1c8fefef 323 motor3->enable();
jackcassa1967 3:24ab1c8fefef 324 sprintf(bufferCommand,"%s %s","OK",line.c_str());
jackcassa1967 3:24ab1c8fefef 325 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 326 }
jackcassa1967 3:24ab1c8fefef 327 else
jackcassa1967 3:24ab1c8fefef 328 {
jackcassa1967 3:24ab1c8fefef 329 sprintf(bufferCommand,"%s %s","KO",line.c_str());
jackcassa1967 3:24ab1c8fefef 330 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 331 }
jackcassa1967 3:24ab1c8fefef 332
jackcassa1967 3:24ab1c8fefef 333
jackcassa1967 3:24ab1c8fefef 334
jackcassa1967 3:24ab1c8fefef 335
jackcassa1967 3:24ab1c8fefef 336
jackcassa1967 3:24ab1c8fefef 337 }
jackcassa1967 3:24ab1c8fefef 338
jackcassa1967 3:24ab1c8fefef 339 if (command=="GOTOA"){
jackcassa1967 3:24ab1c8fefef 340
jackcassa1967 3:24ab1c8fefef 341 int CurrX=motor2->get_position();
jackcassa1967 3:24ab1c8fefef 342 string TO = parser.next();
jackcassa1967 3:24ab1c8fefef 343 int iTo=atoi(TO.c_str());
jackcassa1967 3:24ab1c8fefef 344 int diff=abs(iTo-CurrX);
jackcassa1967 3:24ab1c8fefef 345 if (CurrX>iTo)
jackcassa1967 3:24ab1c8fefef 346 {
jackcassa1967 3:24ab1c8fefef 347 motor2->set_direction(BACKWARD);
jackcassa1967 3:24ab1c8fefef 348 while (true)
jackcassa1967 3:24ab1c8fefef 349 {
jackcassa1967 3:24ab1c8fefef 350 if(diff==0) break;
jackcassa1967 3:24ab1c8fefef 351 if (PinA3==0) break;
jackcassa1967 3:24ab1c8fefef 352 motor2->Step();
jackcassa1967 3:24ab1c8fefef 353 diff=diff-1;
jackcassa1967 3:24ab1c8fefef 354 }
jackcassa1967 3:24ab1c8fefef 355 }
jackcassa1967 3:24ab1c8fefef 356 else if (iTo>CurrX)
jackcassa1967 3:24ab1c8fefef 357 {
jackcassa1967 3:24ab1c8fefef 358 motor2->set_direction(FORWARD);
jackcassa1967 3:24ab1c8fefef 359 while (true)
jackcassa1967 3:24ab1c8fefef 360 {
jackcassa1967 3:24ab1c8fefef 361 if(diff==0) break;
jackcassa1967 3:24ab1c8fefef 362 if (PinA4==0) break;
jackcassa1967 3:24ab1c8fefef 363 motor2->Step();
jackcassa1967 3:24ab1c8fefef 364 diff=diff-1;
jackcassa1967 3:24ab1c8fefef 365 }
jackcassa1967 3:24ab1c8fefef 366
jackcassa1967 3:24ab1c8fefef 367 }
jackcassa1967 3:24ab1c8fefef 368
jackcassa1967 3:24ab1c8fefef 369 int actualX=motor2->get_position();
jackcassa1967 3:24ab1c8fefef 370 unsigned int CurrEle= 0;
jackcassa1967 3:24ab1c8fefef 371 sprintf(bufferCommand,"%s=%d,%d\n","POSX",actualX,CurrEle);
jackcassa1967 3:24ab1c8fefef 372 sendString(bufferCommand);
jackcassa1967 3:24ab1c8fefef 373
jackcassa1967 3:24ab1c8fefef 374 }
jackcassa1967 3:24ab1c8fefef 375
jackcassa1967 3:24ab1c8fefef 376
jackcassa1967 3:24ab1c8fefef 377
jackcassa1967 3:24ab1c8fefef 378
jackcassa1967 3:24ab1c8fefef 379
jackcassa1967 3:24ab1c8fefef 380
bcostm 0:5701b41769fd 381 }
bcostm 0:5701b41769fd 382
jackcassa1967 3:24ab1c8fefef 383
jackcassa1967 3:24ab1c8fefef 384
jackcassa1967 3:24ab1c8fefef 385 /**
jackcassa1967 3:24ab1c8fefef 386 * @brief This is an example of error handler.
jackcassa1967 3:24ab1c8fefef 387 * @param[in] error Number of the error
jackcassa1967 3:24ab1c8fefef 388 * @retval None
jackcassa1967 3:24ab1c8fefef 389 * @note If needed, implement it, and then attach it:
jackcassa1967 3:24ab1c8fefef 390 * + motor->attach_error_handler(&my_error_handler);
jackcassa1967 3:24ab1c8fefef 391 */
jackcassa1967 3:24ab1c8fefef 392 void my_error_handler(uint16_t error)
jackcassa1967 3:24ab1c8fefef 393 {
jackcassa1967 3:24ab1c8fefef 394 /* Printing to the console. */
jackcassa1967 3:24ab1c8fefef 395 printf("Error %d detected\r\n\n", error);
jackcassa1967 3:24ab1c8fefef 396
jackcassa1967 3:24ab1c8fefef 397 /* Infinite loop */
jackcassa1967 3:24ab1c8fefef 398
jackcassa1967 3:24ab1c8fefef 399 }
jackcassa1967 3:24ab1c8fefef 400
jackcassa1967 3:24ab1c8fefef 401
jackcassa1967 3:24ab1c8fefef 402 /* Main ----------------------------------------------------------------------*/
jackcassa1967 3:24ab1c8fefef 403
jackcassa1967 3:24ab1c8fefef 404
jackcassa1967 3:24ab1c8fefef 405 char * debug_r;
bcostm 0:5701b41769fd 406 int main()
bcostm 0:5701b41769fd 407 {
jackcassa1967 3:24ab1c8fefef 408
jackcassa1967 3:24ab1c8fefef 409
jackcassa1967 3:24ab1c8fefef 410 sendString("INFO INT WITH POWER OFF");
jackcassa1967 3:24ab1c8fefef 411 POWER=1;
jackcassa1967 3:24ab1c8fefef 412 wait_ms(4000);
jackcassa1967 3:24ab1c8fefef 413
jackcassa1967 3:24ab1c8fefef 414
jackcassa1967 3:24ab1c8fefef 415
jackcassa1967 3:24ab1c8fefef 416 /*----- Initialization. -----*/
jackcassa1967 3:24ab1c8fefef 417
jackcassa1967 3:24ab1c8fefef 418 /* Initializing SPI bus. */
jackcassa1967 3:24ab1c8fefef 419 DevSPI dev_spi(D11, D12, D13);
jackcassa1967 3:24ab1c8fefef 420
jackcassa1967 3:24ab1c8fefef 421 /* Initializing Motor Control Components. */
jackcassa1967 3:24ab1c8fefef 422 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
jackcassa1967 3:24ab1c8fefef 423 #ifdef M2
jackcassa1967 3:24ab1c8fefef 424 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
jackcassa1967 3:24ab1c8fefef 425 #ifdef M3
jackcassa1967 3:24ab1c8fefef 426 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
jackcassa1967 3:24ab1c8fefef 427 #endif
jackcassa1967 3:24ab1c8fefef 428 #endif
jackcassa1967 3:24ab1c8fefef 429
jackcassa1967 3:24ab1c8fefef 430 motor1->attach_error_handler(my_error_handler);
jackcassa1967 3:24ab1c8fefef 431
jackcassa1967 3:24ab1c8fefef 432 //motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
jackcassa1967 3:24ab1c8fefef 433 // motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
jackcassa1967 3:24ab1c8fefef 434 // motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
jackcassa1967 3:24ab1c8fefef 435
jackcassa1967 3:24ab1c8fefef 436
jackcassa1967 3:24ab1c8fefef 437 if (motor1->init() != COMPONENT_OK) {
jackcassa1967 3:24ab1c8fefef 438 exit(EXIT_FAILURE);
jackcassa1967 3:24ab1c8fefef 439 }
jackcassa1967 3:24ab1c8fefef 440 #ifdef M2
jackcassa1967 3:24ab1c8fefef 441 if (motor2->init() != COMPONENT_OK) {
jackcassa1967 3:24ab1c8fefef 442 exit(EXIT_FAILURE);
jackcassa1967 3:24ab1c8fefef 443 }
jackcassa1967 3:24ab1c8fefef 444 #ifdef M3
jackcassa1967 3:24ab1c8fefef 445 if (motor3->init() != COMPONENT_OK) {
jackcassa1967 3:24ab1c8fefef 446 exit(EXIT_FAILURE);
jackcassa1967 3:24ab1c8fefef 447 }
jackcassa1967 3:24ab1c8fefef 448 #endif
jackcassa1967 3:24ab1c8fefef 449 #endif
jackcassa1967 3:24ab1c8fefef 450
jackcassa1967 3:24ab1c8fefef 451
jackcassa1967 3:24ab1c8fefef 452 motorStepMode_t stepm;
jackcassa1967 3:24ab1c8fefef 453 step_mode=motor1->set_step_mode(StepperMotor::STEP_MODE_1_4);
jackcassa1967 3:24ab1c8fefef 454 #ifdef M2
jackcassa1967 3:24ab1c8fefef 455 step_mode=motor2->set_step_mode(StepperMotor::STEP_MODE_1_8);
jackcassa1967 3:24ab1c8fefef 456 #ifdef M3
jackcassa1967 3:24ab1c8fefef 457 step_mode=motor3->set_step_mode(StepperMotor::STEP_MODE_1_16);
jackcassa1967 3:24ab1c8fefef 458 #endif
jackcassa1967 3:24ab1c8fefef 459 #endif
jackcassa1967 3:24ab1c8fefef 460
jackcassa1967 3:24ab1c8fefef 461 step_mode=motor1->get_parameter(L6474_STEP_MODE);
jackcassa1967 3:24ab1c8fefef 462 #ifdef M2
jackcassa1967 3:24ab1c8fefef 463 step_mode=motor2->get_parameter(L6474_STEP_MODE);
jackcassa1967 3:24ab1c8fefef 464 #ifdef M3
jackcassa1967 3:24ab1c8fefef 465 step_mode=motor3->get_parameter(L6474_STEP_MODE);
jackcassa1967 3:24ab1c8fefef 466 #endif
jackcassa1967 3:24ab1c8fefef 467 #endif
jackcassa1967 3:24ab1c8fefef 468
jackcassa1967 3:24ab1c8fefef 469 /* Printing to the console. */
jackcassa1967 3:24ab1c8fefef 470
jackcassa1967 3:24ab1c8fefef 471
jackcassa1967 3:24ab1c8fefef 472 InterA4.rise(&alarm_pinA4_on);
jackcassa1967 3:24ab1c8fefef 473 InterA4.fall(&alarm_pinA4_off);
jackcassa1967 3:24ab1c8fefef 474 InterA4.enable_irq();
jackcassa1967 3:24ab1c8fefef 475
jackcassa1967 3:24ab1c8fefef 476
jackcassa1967 3:24ab1c8fefef 477 motor2->enable();
jackcassa1967 3:24ab1c8fefef 478 motor3->enable();
jackcassa1967 3:24ab1c8fefef 479
jackcassa1967 3:24ab1c8fefef 480
jackcassa1967 3:24ab1c8fefef 481
jackcassa1967 3:24ab1c8fefef 482
jackcassa1967 3:24ab1c8fefef 483 while(false) {
jackcassa1967 3:24ab1c8fefef 484 motor2->set_direction(FORWARD);
jackcassa1967 3:24ab1c8fefef 485 motor3->set_direction(FORWARD);
jackcassa1967 3:24ab1c8fefef 486 for (int i=0;i<200*16;i++){
jackcassa1967 3:24ab1c8fefef 487 motor2->Step();
jackcassa1967 3:24ab1c8fefef 488 motor3->Step();
jackcassa1967 3:24ab1c8fefef 489 wait_us(300);
jackcassa1967 3:24ab1c8fefef 490 }
jackcassa1967 3:24ab1c8fefef 491
jackcassa1967 3:24ab1c8fefef 492 POWER=0;
jackcassa1967 3:24ab1c8fefef 493 wait_ms(5000);
jackcassa1967 3:24ab1c8fefef 494 POWER=1;
jackcassa1967 3:24ab1c8fefef 495 wait_ms(5000);
jackcassa1967 3:24ab1c8fefef 496
jackcassa1967 3:24ab1c8fefef 497 motor2->enable();
jackcassa1967 3:24ab1c8fefef 498 motor3->enable();
jackcassa1967 3:24ab1c8fefef 499
jackcassa1967 3:24ab1c8fefef 500 motor2->set_direction(BACKWARD);
jackcassa1967 3:24ab1c8fefef 501 motor3->set_direction(BACKWARD);
jackcassa1967 3:24ab1c8fefef 502
jackcassa1967 3:24ab1c8fefef 503 for (int i=0;i<200*16;i++){
jackcassa1967 3:24ab1c8fefef 504 motor2->Step();
jackcassa1967 3:24ab1c8fefef 505 motor3->Step();
jackcassa1967 3:24ab1c8fefef 506 wait_us(300);
jackcassa1967 3:24ab1c8fefef 507 }
jackcassa1967 3:24ab1c8fefef 508
jackcassa1967 3:24ab1c8fefef 509 POWER=0;
jackcassa1967 3:24ab1c8fefef 510 wait_ms(5000);
jackcassa1967 3:24ab1c8fefef 511 POWER=1;
jackcassa1967 3:24ab1c8fefef 512 wait_ms(5000);
jackcassa1967 3:24ab1c8fefef 513
jackcassa1967 3:24ab1c8fefef 514 motor2->enable();
jackcassa1967 3:24ab1c8fefef 515 motor3->enable();
jackcassa1967 3:24ab1c8fefef 516
jackcassa1967 3:24ab1c8fefef 517 }
jackcassa1967 3:24ab1c8fefef 518
jackcassa1967 3:24ab1c8fefef 519 /* Infinite Loop. */
jackcassa1967 3:24ab1c8fefef 520 while(false) {
jackcassa1967 3:24ab1c8fefef 521
jackcassa1967 3:24ab1c8fefef 522 step_mode=motor1->set_step_mode(StepperMotor::STEP_MODE_1_4);
jackcassa1967 3:24ab1c8fefef 523 #ifdef M2
jackcassa1967 3:24ab1c8fefef 524 step_mode=motor2->set_step_mode(StepperMotor::STEP_MODE_1_4);
jackcassa1967 3:24ab1c8fefef 525 #ifdef M3
jackcassa1967 3:24ab1c8fefef 526 step_mode=motor3->set_step_mode(StepperMotor::STEP_MODE_1_4);
jackcassa1967 3:24ab1c8fefef 527 #endif
jackcassa1967 3:24ab1c8fefef 528 #endif
jackcassa1967 3:24ab1c8fefef 529
jackcassa1967 3:24ab1c8fefef 530 /* Requesting to go to a specified position. */
jackcassa1967 3:24ab1c8fefef 531 motor1->go_to(- (STEPS >> 1));
jackcassa1967 3:24ab1c8fefef 532 #ifdef M2
jackcassa1967 3:24ab1c8fefef 533 motor2->go_to(- (STEPS >> 1));
jackcassa1967 3:24ab1c8fefef 534 #ifdef M3
jackcassa1967 3:24ab1c8fefef 535 motor3->go_to(- (STEPS >> 1));
jackcassa1967 3:24ab1c8fefef 536 #endif
jackcassa1967 3:24ab1c8fefef 537 #endif
jackcassa1967 3:24ab1c8fefef 538
jackcassa1967 3:24ab1c8fefef 539 /* Waiting while the motor is active. */
jackcassa1967 3:24ab1c8fefef 540 motor1->wait_while_active();
jackcassa1967 3:24ab1c8fefef 541 #ifdef M2
jackcassa1967 3:24ab1c8fefef 542 motor2->wait_while_active();
jackcassa1967 3:24ab1c8fefef 543 #ifdef M3
jackcassa1967 3:24ab1c8fefef 544 motor3->wait_while_active();
jackcassa1967 3:24ab1c8fefef 545 #endif
jackcassa1967 3:24ab1c8fefef 546 #endif
jackcassa1967 3:24ab1c8fefef 547
jackcassa1967 3:24ab1c8fefef 548 wait_ms(1000);
jackcassa1967 3:24ab1c8fefef 549
jackcassa1967 3:24ab1c8fefef 550 step_mode=motor1->set_step_mode(StepperMotor::STEP_MODE_1_8);
jackcassa1967 3:24ab1c8fefef 551 #ifdef M2
jackcassa1967 3:24ab1c8fefef 552 step_mode=motor2->set_step_mode(StepperMotor::STEP_MODE_1_16);
jackcassa1967 3:24ab1c8fefef 553 #ifdef M3
jackcassa1967 3:24ab1c8fefef 554 step_mode=motor3->set_step_mode(StepperMotor::STEP_MODE_1_4);
jackcassa1967 3:24ab1c8fefef 555 #endif
jackcassa1967 3:24ab1c8fefef 556 #endif
jackcassa1967 3:24ab1c8fefef 557 /* Requesting to go to a specified position. */
jackcassa1967 3:24ab1c8fefef 558 motor1->go_to( (STEPS >> 1));
jackcassa1967 3:24ab1c8fefef 559 #ifdef M2
jackcassa1967 3:24ab1c8fefef 560 motor2->go_to( (STEPS >> 1));
jackcassa1967 3:24ab1c8fefef 561 #ifdef M3
jackcassa1967 3:24ab1c8fefef 562 motor3->go_to( (STEPS >> 1));
jackcassa1967 3:24ab1c8fefef 563 #endif
jackcassa1967 3:24ab1c8fefef 564 #endif
jackcassa1967 3:24ab1c8fefef 565
jackcassa1967 3:24ab1c8fefef 566 /* Waiting while the motor is active. */
jackcassa1967 3:24ab1c8fefef 567 motor1->wait_while_active();
jackcassa1967 3:24ab1c8fefef 568 #ifdef M2
jackcassa1967 3:24ab1c8fefef 569 motor2->wait_while_active();
jackcassa1967 3:24ab1c8fefef 570 #ifdef M3
jackcassa1967 3:24ab1c8fefef 571 motor3->wait_while_active();
jackcassa1967 3:24ab1c8fefef 572 #endif
jackcassa1967 3:24ab1c8fefef 573 #endif
jackcassa1967 3:24ab1c8fefef 574
jackcassa1967 3:24ab1c8fefef 575
jackcassa1967 3:24ab1c8fefef 576 }
jackcassa1967 3:24ab1c8fefef 577
jackcassa1967 3:24ab1c8fefef 578 string line;
jackcassa1967 3:24ab1c8fefef 579
jackcassa1967 3:24ab1c8fefef 580
jackcassa1967 3:24ab1c8fefef 581
jackcassa1967 3:24ab1c8fefef 582
jackcassa1967 3:24ab1c8fefef 583 while(true){
jackcassa1967 3:24ab1c8fefef 584
jackcassa1967 3:24ab1c8fefef 585 if ( Console.readable() )
jackcassa1967 3:24ab1c8fefef 586 {
jackcassa1967 3:24ab1c8fefef 587 debug_r= (char*) line.c_str();
jackcassa1967 3:24ab1c8fefef 588
jackcassa1967 3:24ab1c8fefef 589 char serial_char = Console.getc();
jackcassa1967 3:24ab1c8fefef 590 serial_count++;
jackcassa1967 3:24ab1c8fefef 591 if (serial_char == '\n' )
jackcassa1967 3:24ab1c8fefef 592 {
jackcassa1967 3:24ab1c8fefef 593 // pc.puts(line.c_str());
jackcassa1967 3:24ab1c8fefef 594 processLine(line);
jackcassa1967 3:24ab1c8fefef 595 line="";
jackcassa1967 3:24ab1c8fefef 596 serial_count=0;
jackcassa1967 3:24ab1c8fefef 597
jackcassa1967 3:24ab1c8fefef 598 }
jackcassa1967 3:24ab1c8fefef 599 else
jackcassa1967 3:24ab1c8fefef 600 {
jackcassa1967 3:24ab1c8fefef 601 line.push_back(serial_char);
jackcassa1967 3:24ab1c8fefef 602 }
jackcassa1967 3:24ab1c8fefef 603
jackcassa1967 3:24ab1c8fefef 604 }
jackcassa1967 3:24ab1c8fefef 605
jackcassa1967 3:24ab1c8fefef 606 }
jackcassa1967 3:24ab1c8fefef 607 }
bcostm 2:35f13b7f3659 608