masayuki shindo
/
L6470Stepper_test4
consept_meter
main.cpp@1:39b4d6e71f76, 2012-11-06 (annotated)
- Committer:
- junTMUG
- Date:
- Tue Nov 06 11:22:47 2012 +0000
- Revision:
- 1:39b4d6e71f76
- Parent:
- 0:134db3267830
- Child:
- 2:824d7df88ed9
type1_speed?type1_tachp?Thread?
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
shindo | 0:134db3267830 | 1 | #include "mbed.h" |
junTMUG | 1:39b4d6e71f76 | 2 | #include "rtos.h" |
shindo | 0:134db3267830 | 3 | |
shindo | 0:134db3267830 | 4 | DigitalIn busy1(p26); |
shindo | 0:134db3267830 | 5 | DigitalIn flag1(p25); |
shindo | 0:134db3267830 | 6 | DigitalOut cs1(p8); |
shindo | 0:134db3267830 | 7 | DigitalOut stck1(p28); |
shindo | 0:134db3267830 | 8 | DigitalOut stby1(p27); |
shindo | 0:134db3267830 | 9 | |
shindo | 0:134db3267830 | 10 | DigitalIn busy2(p22); |
shindo | 0:134db3267830 | 11 | DigitalIn flag2(p21); |
shindo | 0:134db3267830 | 12 | DigitalOut cs2(p19); |
shindo | 0:134db3267830 | 13 | DigitalOut stck2(p24); |
shindo | 0:134db3267830 | 14 | DigitalOut stby2(p23); |
shindo | 0:134db3267830 | 15 | |
shindo | 0:134db3267830 | 16 | DigitalOut led1(LED1); |
shindo | 0:134db3267830 | 17 | DigitalOut led2(LED2); |
shindo | 0:134db3267830 | 18 | DigitalOut led3(LED3); |
shindo | 0:134db3267830 | 19 | DigitalOut led4(LED4); |
shindo | 0:134db3267830 | 20 | |
shindo | 0:134db3267830 | 21 | SPI spi(p5, p6, p7); // mosi, miso, sclk |
junTMUG | 1:39b4d6e71f76 | 22 | Mutex motor1_mutex, motor2_mutex; |
shindo | 0:134db3267830 | 23 | |
shindo | 0:134db3267830 | 24 | void writeCommand(int port, unsigned char data) |
shindo | 0:134db3267830 | 25 | { |
shindo | 0:134db3267830 | 26 | if (port == 1) { |
shindo | 0:134db3267830 | 27 | cs1 = 0; |
shindo | 0:134db3267830 | 28 | spi.write(data); |
shindo | 0:134db3267830 | 29 | cs1 = 1; |
shindo | 0:134db3267830 | 30 | } else { |
shindo | 0:134db3267830 | 31 | cs2 = 0; |
shindo | 0:134db3267830 | 32 | spi.write(data); |
shindo | 0:134db3267830 | 33 | cs2 = 1; |
shindo | 0:134db3267830 | 34 | } |
shindo | 0:134db3267830 | 35 | } |
shindo | 0:134db3267830 | 36 | |
junTMUG | 1:39b4d6e71f76 | 37 | void writeCommand4(int port, unsigned int data) |
junTMUG | 1:39b4d6e71f76 | 38 | { |
junTMUG | 1:39b4d6e71f76 | 39 | if (port == 1) |
junTMUG | 1:39b4d6e71f76 | 40 | motor1_mutex.lock(); |
junTMUG | 1:39b4d6e71f76 | 41 | else |
junTMUG | 1:39b4d6e71f76 | 42 | motor2_mutex.lock(); |
junTMUG | 1:39b4d6e71f76 | 43 | writeCommand(port, data >> 24); |
junTMUG | 1:39b4d6e71f76 | 44 | writeCommand(port, (data >> 16) & 0xff); |
junTMUG | 1:39b4d6e71f76 | 45 | writeCommand(port, (data >> 8) & 0xff); |
junTMUG | 1:39b4d6e71f76 | 46 | writeCommand(port, data & 0xff); |
junTMUG | 1:39b4d6e71f76 | 47 | if (port == 1) |
junTMUG | 1:39b4d6e71f76 | 48 | motor1_mutex.unlock(); |
junTMUG | 1:39b4d6e71f76 | 49 | else |
junTMUG | 1:39b4d6e71f76 | 50 | motor2_mutex.unlock(); |
junTMUG | 1:39b4d6e71f76 | 51 | } |
junTMUG | 1:39b4d6e71f76 | 52 | |
shindo | 0:134db3267830 | 53 | unsigned char readCommand(int port) |
shindo | 0:134db3267830 | 54 | { |
shindo | 0:134db3267830 | 55 | unsigned char result = 0; |
shindo | 0:134db3267830 | 56 | |
shindo | 0:134db3267830 | 57 | if (port == 1) { |
shindo | 0:134db3267830 | 58 | cs1 = 0; |
shindo | 0:134db3267830 | 59 | result = spi.write(0); |
shindo | 0:134db3267830 | 60 | cs1 = 1; |
shindo | 0:134db3267830 | 61 | } else { |
shindo | 0:134db3267830 | 62 | cs2 = 0; |
shindo | 0:134db3267830 | 63 | result = spi.write(0); |
shindo | 0:134db3267830 | 64 | cs2 = 1; |
shindo | 0:134db3267830 | 65 | } |
shindo | 0:134db3267830 | 66 | |
shindo | 0:134db3267830 | 67 | return result; |
shindo | 0:134db3267830 | 68 | } |
shindo | 0:134db3267830 | 69 | |
shindo | 0:134db3267830 | 70 | void initMotor() |
shindo | 0:134db3267830 | 71 | { |
shindo | 0:134db3267830 | 72 | spi.format(8,0); |
shindo | 0:134db3267830 | 73 | busy1.mode(PullUp); |
shindo | 0:134db3267830 | 74 | flag1.mode(PullUp); |
shindo | 0:134db3267830 | 75 | busy2.mode(PullUp); |
shindo | 0:134db3267830 | 76 | flag2.mode(PullUp); |
shindo | 0:134db3267830 | 77 | stby1 = 1; |
shindo | 0:134db3267830 | 78 | stby2 = 1; |
shindo | 0:134db3267830 | 79 | for (int port = 1; port <= 2; port++) { |
shindo | 0:134db3267830 | 80 | |
shindo | 0:134db3267830 | 81 | writeCommand(port, 0x08); |
shindo | 0:134db3267830 | 82 | writeCommand(port, 0x00); |
shindo | 0:134db3267830 | 83 | |
shindo | 0:134db3267830 | 84 | writeCommand(port, 0x09); |
shindo | 0:134db3267830 | 85 | writeCommand(port, 0xff); |
shindo | 0:134db3267830 | 86 | |
shindo | 0:134db3267830 | 87 | writeCommand(port, 0x0a); |
shindo | 0:134db3267830 | 88 | writeCommand(port, 0xff); |
shindo | 0:134db3267830 | 89 | |
shindo | 0:134db3267830 | 90 | writeCommand(port, 0x0b); |
shindo | 0:134db3267830 | 91 | writeCommand(port, 0xff); |
shindo | 0:134db3267830 | 92 | |
shindo | 0:134db3267830 | 93 | writeCommand(port, 0x0c); |
shindo | 0:134db3267830 | 94 | writeCommand(port, 0xff); |
shindo | 0:134db3267830 | 95 | |
shindo | 0:134db3267830 | 96 | writeCommand(port, 0x13); |
shindo | 0:134db3267830 | 97 | writeCommand(port, 0x0f); |
shindo | 0:134db3267830 | 98 | |
shindo | 0:134db3267830 | 99 | writeCommand(port, 0x14); |
shindo | 0:134db3267830 | 100 | writeCommand(port, 0x7f); |
shindo | 0:134db3267830 | 101 | } |
shindo | 0:134db3267830 | 102 | } |
junTMUG | 1:39b4d6e71f76 | 103 | |
shindo | 0:134db3267830 | 104 | void MAXspeed_port_1_A() |
shindo | 0:134db3267830 | 105 | { |
shindo | 0:134db3267830 | 106 | writeCommand(1, 0x07); |
shindo | 0:134db3267830 | 107 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 108 | writeCommand(1, 0x32); |
shindo | 0:134db3267830 | 109 | } |
junTMUG | 1:39b4d6e71f76 | 110 | |
shindo | 0:134db3267830 | 111 | void Step_Startspeed_port_1_A() |
shindo | 0:134db3267830 | 112 | { |
shindo | 0:134db3267830 | 113 | writeCommand(1, 0x05); |
shindo | 0:134db3267830 | 114 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 115 | writeCommand(1, 0x23); |
shindo | 0:134db3267830 | 116 | } |
junTMUG | 1:39b4d6e71f76 | 117 | |
shindo | 0:134db3267830 | 118 | void Step_Stopspeed_port_1_A() |
shindo | 0:134db3267830 | 119 | { |
shindo | 0:134db3267830 | 120 | writeCommand(1, 0x06); |
shindo | 0:134db3267830 | 121 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 122 | writeCommand(1, 0x15); |
shindo | 0:134db3267830 | 123 | } |
junTMUG | 1:39b4d6e71f76 | 124 | |
shindo | 0:134db3267830 | 125 | void CW_port_1_A() |
shindo | 0:134db3267830 | 126 | { |
shindo | 0:134db3267830 | 127 | writeCommand(1, 0x51); |
shindo | 0:134db3267830 | 128 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 129 | writeCommand(1, 0x25); |
shindo | 0:134db3267830 | 130 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 131 | } |
junTMUG | 1:39b4d6e71f76 | 132 | |
shindo | 0:134db3267830 | 133 | void CCW_port_1_A() |
shindo | 0:134db3267830 | 134 | { |
shindo | 0:134db3267830 | 135 | writeCommand(1, 0x50); |
shindo | 0:134db3267830 | 136 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 137 | writeCommand(1, 0x10); |
shindo | 0:134db3267830 | 138 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 139 | } |
junTMUG | 1:39b4d6e71f76 | 140 | |
shindo | 0:134db3267830 | 141 | void CCW_port_1_A2() |
shindo | 0:134db3267830 | 142 | { |
shindo | 0:134db3267830 | 143 | writeCommand(1, 0x50); |
shindo | 0:134db3267830 | 144 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 145 | writeCommand(1, 0x40); |
shindo | 0:134db3267830 | 146 | writeCommand(1, 0x00); |
shindo | 0:134db3267830 | 147 | } |
shindo | 0:134db3267830 | 148 | //---------------------------------------------- |
shindo | 0:134db3267830 | 149 | void MAXspeed_port_2_A() |
shindo | 0:134db3267830 | 150 | { |
shindo | 0:134db3267830 | 151 | writeCommand(2, 0x07); |
shindo | 0:134db3267830 | 152 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 153 | writeCommand(2, 0x32); |
shindo | 0:134db3267830 | 154 | } |
junTMUG | 1:39b4d6e71f76 | 155 | |
shindo | 0:134db3267830 | 156 | void Step_Startspeed_port_2_A() |
shindo | 0:134db3267830 | 157 | { |
shindo | 0:134db3267830 | 158 | writeCommand(2, 0x05); |
shindo | 0:134db3267830 | 159 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 160 | writeCommand(2, 0x23); |
shindo | 0:134db3267830 | 161 | } |
junTMUG | 1:39b4d6e71f76 | 162 | |
shindo | 0:134db3267830 | 163 | void Step_Stopspeed_port_2_A() |
shindo | 0:134db3267830 | 164 | { |
shindo | 0:134db3267830 | 165 | writeCommand(2, 0x06); |
shindo | 0:134db3267830 | 166 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 167 | writeCommand(2, 0x15); |
shindo | 0:134db3267830 | 168 | } |
junTMUG | 1:39b4d6e71f76 | 169 | |
shindo | 0:134db3267830 | 170 | void CW_port_2_A() |
shindo | 0:134db3267830 | 171 | { |
shindo | 0:134db3267830 | 172 | writeCommand(2, 0x51); |
shindo | 0:134db3267830 | 173 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 174 | writeCommand(2, 0x25); |
shindo | 0:134db3267830 | 175 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 176 | } |
junTMUG | 1:39b4d6e71f76 | 177 | |
shindo | 0:134db3267830 | 178 | void CCW_port_2_A() |
shindo | 0:134db3267830 | 179 | { |
shindo | 0:134db3267830 | 180 | writeCommand(2, 0x50); |
shindo | 0:134db3267830 | 181 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 182 | writeCommand(2, 0x10); |
shindo | 0:134db3267830 | 183 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 184 | } |
junTMUG | 1:39b4d6e71f76 | 185 | |
shindo | 0:134db3267830 | 186 | void CCW_port_2_A2() |
shindo | 0:134db3267830 | 187 | { |
shindo | 0:134db3267830 | 188 | writeCommand(2, 0x50); |
shindo | 0:134db3267830 | 189 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 190 | writeCommand(2, 0x40); |
shindo | 0:134db3267830 | 191 | writeCommand(2, 0x00); |
shindo | 0:134db3267830 | 192 | } |
junTMUG | 1:39b4d6e71f76 | 193 | |
shindo | 0:134db3267830 | 194 | //----------------------------------- |
junTMUG | 1:39b4d6e71f76 | 195 | void startup1 () |
shindo | 0:134db3267830 | 196 | { |
shindo | 0:134db3267830 | 197 | CW_port_1_A(); |
shindo | 0:134db3267830 | 198 | CW_port_2_A(); |
shindo | 0:134db3267830 | 199 | } |
junTMUG | 1:39b4d6e71f76 | 200 | |
junTMUG | 1:39b4d6e71f76 | 201 | void startup2 () |
shindo | 0:134db3267830 | 202 | { |
shindo | 0:134db3267830 | 203 | CCW_port_1_A(); |
shindo | 0:134db3267830 | 204 | CCW_port_2_A(); |
shindo | 0:134db3267830 | 205 | } |
junTMUG | 1:39b4d6e71f76 | 206 | |
shindo | 0:134db3267830 | 207 | void power_up_p1_p2 () |
shindo | 0:134db3267830 | 208 | { |
junTMUG | 1:39b4d6e71f76 | 209 | writeCommand4(1, 0x51004000); |
junTMUG | 1:39b4d6e71f76 | 210 | writeCommand4(2, 0x51004000); |
shindo | 0:134db3267830 | 211 | wait_ms (630); |
shindo | 0:134db3267830 | 212 | writeCommand(2, 0xb8); |
shindo | 0:134db3267830 | 213 | wait_ms (175); |
shindo | 0:134db3267830 | 214 | writeCommand(1, 0xb8); |
shindo | 0:134db3267830 | 215 | } |
junTMUG | 1:39b4d6e71f76 | 216 | |
shindo | 0:134db3267830 | 217 | void on () |
shindo | 0:134db3267830 | 218 | { |
shindo | 0:134db3267830 | 219 | CW_port_2_A(); |
shindo | 0:134db3267830 | 220 | wait_ms(200); |
shindo | 0:134db3267830 | 221 | writeCommand(2, 0xb8); |
shindo | 0:134db3267830 | 222 | } |
junTMUG | 1:39b4d6e71f76 | 223 | |
shindo | 0:134db3267830 | 224 | void reb1 () |
shindo | 0:134db3267830 | 225 | { |
shindo | 0:134db3267830 | 226 | CW_port_2_A(); |
shindo | 0:134db3267830 | 227 | wait_ms(331); |
shindo | 0:134db3267830 | 228 | writeCommand(2, 0xb0); |
shindo | 0:134db3267830 | 229 | CCW_port_2_A(); |
shindo | 0:134db3267830 | 230 | wait_ms(1400); |
shindo | 0:134db3267830 | 231 | writeCommand(2, 0xb0); |
shindo | 0:134db3267830 | 232 | wait_ms(300); |
shindo | 0:134db3267830 | 233 | } |
junTMUG | 1:39b4d6e71f76 | 234 | |
shindo | 0:134db3267830 | 235 | void reb2 () |
shindo | 0:134db3267830 | 236 | { |
junTMUG | 1:39b4d6e71f76 | 237 | writeCommand4(2, 0x51003000); |
shindo | 0:134db3267830 | 238 | wait_ms(60); |
shindo | 0:134db3267830 | 239 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 240 | writeCommand4(2, 0x50003000); |
junTMUG | 1:39b4d6e71f76 | 241 | |
shindo | 0:134db3267830 | 242 | wait_ms(60); |
shindo | 0:134db3267830 | 243 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 244 | writeCommand4(2, 0x51003000); |
shindo | 0:134db3267830 | 245 | wait_ms(60); |
shindo | 0:134db3267830 | 246 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 247 | writeCommand4(2, 0x50003000); |
shindo | 0:134db3267830 | 248 | wait_ms(60); |
shindo | 0:134db3267830 | 249 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 250 | writeCommand4(2, 0x51003000); |
shindo | 0:134db3267830 | 251 | wait_ms(60); |
shindo | 0:134db3267830 | 252 | } |
junTMUG | 1:39b4d6e71f76 | 253 | |
shindo | 0:134db3267830 | 254 | void reb3 () |
shindo | 0:134db3267830 | 255 | { |
junTMUG | 1:39b4d6e71f76 | 256 | writeCommand4(2, 0x51001500); |
shindo | 0:134db3267830 | 257 | wait_ms(500); |
shindo | 0:134db3267830 | 258 | |
junTMUG | 1:39b4d6e71f76 | 259 | writeCommand4(2, 0x50000300); |
shindo | 0:134db3267830 | 260 | wait_ms(4000); |
shindo | 0:134db3267830 | 261 | writeCommand(2, 0xb0); |
shindo | 0:134db3267830 | 262 | } |
shindo | 0:134db3267830 | 263 | |
junTMUG | 1:39b4d6e71f76 | 264 | void type1_tacho (void const *argument) |
junTMUG | 1:39b4d6e71f76 | 265 | { |
junTMUG | 1:39b4d6e71f76 | 266 | Thread::wait(2000); |
junTMUG | 1:39b4d6e71f76 | 267 | |
junTMUG | 1:39b4d6e71f76 | 268 | writeCommand4(2, 0x51001000); |
junTMUG | 1:39b4d6e71f76 | 269 | Thread::wait(500); |
shindo | 0:134db3267830 | 270 | |
shindo | 0:134db3267830 | 271 | writeCommand(2, 0xb0); |
junTMUG | 1:39b4d6e71f76 | 272 | Thread::wait(100); |
shindo | 0:134db3267830 | 273 | |
junTMUG | 1:39b4d6e71f76 | 274 | writeCommand4(2, 0x51002500); |
junTMUG | 1:39b4d6e71f76 | 275 | Thread::wait(380); |
shindo | 0:134db3267830 | 276 | reb2 (); |
shindo | 0:134db3267830 | 277 | |
junTMUG | 1:39b4d6e71f76 | 278 | writeCommand4(2, 0x50002000); |
junTMUG | 1:39b4d6e71f76 | 279 | Thread::wait(500); |
shindo | 0:134db3267830 | 280 | writeCommand(2, 0xb0); |
shindo | 0:134db3267830 | 281 | |
junTMUG | 1:39b4d6e71f76 | 282 | writeCommand4(2, 0x51000800); |
junTMUG | 1:39b4d6e71f76 | 283 | Thread::wait(2050); |
shindo | 0:134db3267830 | 284 | |
junTMUG | 1:39b4d6e71f76 | 285 | writeCommand4(2, 0x50002000); |
junTMUG | 1:39b4d6e71f76 | 286 | Thread::wait(450); |
shindo | 0:134db3267830 | 287 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 288 | Thread::wait(200); |
junTMUG | 1:39b4d6e71f76 | 289 | writeCommand4(2, 0x51000500);//---2--- |
junTMUG | 1:39b4d6e71f76 | 290 | Thread::wait(1550); |
shindo | 0:134db3267830 | 291 | |
junTMUG | 1:39b4d6e71f76 | 292 | writeCommand4(2, 0x50002000); |
junTMUG | 1:39b4d6e71f76 | 293 | Thread::wait(430); |
shindo | 0:134db3267830 | 294 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 295 | Thread::wait(200); |
junTMUG | 1:39b4d6e71f76 | 296 | writeCommand4(2, 0x51000400);//---3--- |
junTMUG | 1:39b4d6e71f76 | 297 | Thread::wait(2000); |
shindo | 0:134db3267830 | 298 | |
junTMUG | 1:39b4d6e71f76 | 299 | writeCommand4(2, 0x50002000); |
junTMUG | 1:39b4d6e71f76 | 300 | Thread::wait(400); |
shindo | 0:134db3267830 | 301 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 302 | Thread::wait(200); |
junTMUG | 1:39b4d6e71f76 | 303 | writeCommand4(2, 0x51000200);//---4--- |
junTMUG | 1:39b4d6e71f76 | 304 | Thread::wait(4000); |
shindo | 0:134db3267830 | 305 | |
junTMUG | 1:39b4d6e71f76 | 306 | writeCommand4(2, 0x50002000); |
junTMUG | 1:39b4d6e71f76 | 307 | Thread::wait(300); |
shindo | 0:134db3267830 | 308 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 309 | Thread::wait(200); |
junTMUG | 1:39b4d6e71f76 | 310 | writeCommand4(2, 0x51000150);//---5--- |
junTMUG | 1:39b4d6e71f76 | 311 | Thread::wait(3000); |
junTMUG | 1:39b4d6e71f76 | 312 | writeCommand4(2, 0x51000100);//---5--- |
junTMUG | 1:39b4d6e71f76 | 313 | Thread::wait(1500); |
shindo | 0:134db3267830 | 314 | |
junTMUG | 1:39b4d6e71f76 | 315 | writeCommand4(2, 0x50002000); |
junTMUG | 1:39b4d6e71f76 | 316 | Thread::wait(300); |
shindo | 0:134db3267830 | 317 | writeCommand(2, 0xb8); |
junTMUG | 1:39b4d6e71f76 | 318 | Thread::wait(200); |
junTMUG | 1:39b4d6e71f76 | 319 | writeCommand4(2, 0x51000150);//---6--- |
junTMUG | 1:39b4d6e71f76 | 320 | Thread::wait(3000); |
junTMUG | 1:39b4d6e71f76 | 321 | writeCommand4(2, 0x51000100);//---6--- |
junTMUG | 1:39b4d6e71f76 | 322 | Thread::wait(2500); |
shindo | 0:134db3267830 | 323 | } |
junTMUG | 1:39b4d6e71f76 | 324 | |
junTMUG | 1:39b4d6e71f76 | 325 | void type1_speed (void const *argument) |
shindo | 0:134db3267830 | 326 | { |
junTMUG | 1:39b4d6e71f76 | 327 | writeCommand4(1, 0x510004b0); |
junTMUG | 1:39b4d6e71f76 | 328 | Thread::wait(1500); |
junTMUG | 1:39b4d6e71f76 | 329 | writeCommand4(1, 0x510003e8); |
junTMUG | 1:39b4d6e71f76 | 330 | Thread::wait(2000); |
junTMUG | 1:39b4d6e71f76 | 331 | writeCommand4(1, 0x51000258); |
junTMUG | 1:39b4d6e71f76 | 332 | Thread::wait(2500); |
junTMUG | 1:39b4d6e71f76 | 333 | writeCommand4(1, 0x51000190); |
junTMUG | 1:39b4d6e71f76 | 334 | Thread::wait(3000); |
junTMUG | 1:39b4d6e71f76 | 335 | writeCommand4(1, 0x5100012c); |
junTMUG | 1:39b4d6e71f76 | 336 | Thread::wait(3500); |
junTMUG | 1:39b4d6e71f76 | 337 | writeCommand4(1, 0x510000fa); |
junTMUG | 1:39b4d6e71f76 | 338 | Thread::wait(4500); |
junTMUG | 1:39b4d6e71f76 | 339 | writeCommand4(1, 0x51000078); |
junTMUG | 1:39b4d6e71f76 | 340 | Thread::wait(5200); |
shindo | 0:134db3267830 | 341 | writeCommand(1, 0xb0); |
shindo | 0:134db3267830 | 342 | } |
shindo | 0:134db3267830 | 343 | |
shindo | 0:134db3267830 | 344 | int main() |
shindo | 0:134db3267830 | 345 | { |
shindo | 0:134db3267830 | 346 | initMotor(); |
shindo | 0:134db3267830 | 347 | MAXspeed_port_1_A(); |
shindo | 0:134db3267830 | 348 | Step_Startspeed_port_1_A(); |
shindo | 0:134db3267830 | 349 | Step_Stopspeed_port_1_A(); |
shindo | 0:134db3267830 | 350 | |
shindo | 0:134db3267830 | 351 | MAXspeed_port_2_A(); |
shindo | 0:134db3267830 | 352 | Step_Startspeed_port_2_A(); |
shindo | 0:134db3267830 | 353 | Step_Stopspeed_port_2_A(); |
shindo | 0:134db3267830 | 354 | |
shindo | 0:134db3267830 | 355 | writeCommand(2, 0x83); |
shindo | 0:134db3267830 | 356 | writeCommand(1, 0x83); |
shindo | 0:134db3267830 | 357 | |
shindo | 0:134db3267830 | 358 | int i = 0; |
shindo | 0:134db3267830 | 359 | while(1) { |
shindo | 0:134db3267830 | 360 | if (i == 0) { |
junTMUG | 1:39b4d6e71f76 | 361 | startup1 (); |
shindo | 0:134db3267830 | 362 | wait_ms(200); |
junTMUG | 1:39b4d6e71f76 | 363 | startup2 (); |
shindo | 0:134db3267830 | 364 | wait_ms(4000); |
shindo | 0:134db3267830 | 365 | } else if (i == 1) { |
shindo | 0:134db3267830 | 366 | power_up_p1_p2 (); |
shindo | 0:134db3267830 | 367 | wait_ms(1000); |
shindo | 0:134db3267830 | 368 | } else if (i == 2) { |
shindo | 0:134db3267830 | 369 | CCW_port_1_A2(); |
shindo | 0:134db3267830 | 370 | CCW_port_2_A2(); |
shindo | 0:134db3267830 | 371 | wait_ms(1000); |
shindo | 0:134db3267830 | 372 | } else if (i == 3) { |
shindo | 0:134db3267830 | 373 | on (); |
shindo | 0:134db3267830 | 374 | wait_ms(2000); |
shindo | 0:134db3267830 | 375 | } else if (i == 4) { |
shindo | 0:134db3267830 | 376 | reb1 (); |
shindo | 0:134db3267830 | 377 | } else if (i == 5) { |
junTMUG | 1:39b4d6e71f76 | 378 | Thread speed_thread(&type1_speed); |
junTMUG | 1:39b4d6e71f76 | 379 | Thread tacho_thread(&type1_tacho); |
shindo | 0:134db3267830 | 380 | } else if (i == 6) { |
junTMUG | 1:39b4d6e71f76 | 381 | writeCommand4(1, 0x5000012c); |
shindo | 0:134db3267830 | 382 | |
junTMUG | 1:39b4d6e71f76 | 383 | writeCommand4(2, 0x50000300); |
shindo | 0:134db3267830 | 384 | wait_ms(5000); |
shindo | 0:134db3267830 | 385 | reb3 (); |
shindo | 0:134db3267830 | 386 | reb3 (); |
shindo | 0:134db3267830 | 387 | reb3 (); |
shindo | 0:134db3267830 | 388 | reb3 (); |
shindo | 0:134db3267830 | 389 | reb3 (); |
shindo | 0:134db3267830 | 390 | |
junTMUG | 1:39b4d6e71f76 | 391 | writeCommand4(2, 0x50000300); |
shindo | 0:134db3267830 | 392 | wait_ms(1800); |
shindo | 0:134db3267830 | 393 | writeCommand(1, 0xb8); |
shindo | 0:134db3267830 | 394 | writeCommand(2, 0xb8); |
shindo | 0:134db3267830 | 395 | wait(10); |
shindo | 0:134db3267830 | 396 | |
shindo | 0:134db3267830 | 397 | } else if (i == 8) { |
shindo | 0:134db3267830 | 398 | CCW_port_1_A(); |
shindo | 0:134db3267830 | 399 | CCW_port_2_A(); |
shindo | 0:134db3267830 | 400 | wait_ms(5000); |
shindo | 0:134db3267830 | 401 | i = -1; |
shindo | 0:134db3267830 | 402 | } |
shindo | 0:134db3267830 | 403 | wait_ms(200); |
shindo | 0:134db3267830 | 404 | |
shindo | 0:134db3267830 | 405 | i++; |
shindo | 0:134db3267830 | 406 | } |
shindo | 0:134db3267830 | 407 | } |
shindo | 0:134db3267830 | 408 | |
shindo | 0:134db3267830 | 409 | |
shindo | 0:134db3267830 | 410 | |
shindo | 0:134db3267830 | 411 | |
shindo | 0:134db3267830 | 412 |