![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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>© 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 |