stepper.c


1
#define F_CPU 16000000UL

2
3
#include <avr/io.h>

4
#include <stdlib.h>

5
#include <stdio.h>

6
#include <stdint.h>

7
#include <string.h>

8
#include <avr/interrupt.h>

9
#include <math.h>

10
#include "ioport.h"

11
#include "stepper.h"

12
#include "pinout.h"

13
#include "uart.h"

14
#include <util/delay.h>

15
16
uint8_t  debugging = 0;                      //DEBUG-VARIABLE - Meldungen einblenden (1)

17
uint8_t  first_home = 1;

18
uint8_t  first_calc = 1;

19
20
uint8_t stepper_command;                    //1= forward, 0= reverse

21
uint8_t stepper_dir;                      //1= forward, 0= reverse (Ri. HOME)

22
uint8_t akt_dir;

23
24
#define STEPPER_OFF_DELAY 100

25
26
uint8_t stepper_delay = 1;

27
uint8_t stepper_d;

28
uint8_t stepper_max_min = 0;

29
uint8_t stepper_home = 0;

30
uint8_t stepper_rev_home = 0;

31
32
uint16_t stepper_off_delay;

33
34
uint16_t step_counter = 0;

35
uint16_t position_request;

36
uint16_t delta = 0;

37
uint16_t ticks_rampe;

38
uint16_t ticks_rampe_beschl;

39
uint16_t ticks_rampe_brems;

40
uint16_t ticks_rampe_akt;

41
42
volatile uint8_t anzahl_overflows;

43
volatile uint8_t rest_ticks;

44
volatile uint8_t tick_abgelaufen;

45
46
char string[256];

47
char Ausgabestring[30];

48
49
50
51
52
uint16_t Stepper_max = 4300;                    //Eigentlich 4276 - aber geht sowieso nur bis Anschlag

53
54
uint32_t Beschleunigung_Schritte;

55
uint32_t Bremsung_Schritte;

56
float   Schritt_Faktor;

57
float   Beschleunigung_Zeit;

58
float   Bremsung_Zeit;

59
uint64_t Frequenz;

60
float   Beschleunigung_Multiplikator;

61
float   Bremsung_Multiplikator;

62
float   Multiplikator_angepasst;

63
float   Timer_Ticks_1;

64
uint16_t Timer_Ticks_1_int;

65
float   Timer_Ticks_n;

66
float   Timer_Ticks_old;

67
uint16_t Timer_Ticks_n_int;

68
float   Q_Faktor;

69
70
71
72
73
74
75
76
ISR(TIMER2_COMP_vect)                  //Hier wird der Rest der Tick-Wartezeit durchlaufen und das Verarbeitungs-Flag gesetzt

77
{                          

78
  TIMSK &= ~(1<<OCIE2);                //Compare Interrupt deaktivieren  

79
  tick_abgelaufen = 1;  

80
}

81
82
83
ISR(TIMER2_OVF_vect)                  //Da wir nur einen 8-Bit-Timer haben durchlaufen wir mehrere Durchgänge

84
{

85
  anzahl_overflows --;

86
  

87
  PORTB ^= (1<<PB3);

88
  

89
  if(!anzahl_overflows)

90
  {

91
    TIMSK &= ~(1<<TOIE2);              //Timer Overflow Interrupt deaktivieren      

92
    OCR2 = rest_ticks;

93
    TCNT2 = 0;                    //Zähler auf 0

94
    TIMSK |= (1<<OCIE2);              //Compare Interrupt aktivieren  

95
    TIFR  |= (1<<OCF2);                //Output Compare Match Flag löschen (Zähler scharfschalten)

96
  }

97
}

98
99
100
void timer2_init (uint16_t u_ticks)            //Wird für jede Tick-Warterunde gestartet

101
{

102
  tick_abgelaufen = 0;

103
  anzahl_overflows = u_ticks / 256;

104
105
  PORTB ^= (1<<PB2);

106
107
  TCCR2 = (1<<CS22);                  //Takt/64

108
  TCNT2 = 0;                      //Zähler auf 0

109
  TIMSK |= (1<<TOIE2);                //Timer Overflow Interrupt aktivieren  

110
  TIFR  |= (1<<TOV2);                  //Timer Overflow Flag löschen (Zähler scharfschalten)

111
}

112
113
114
115
void update_stepper()

116
{

117
  

118
  static uint8_t stepper_run = 0;

119
  

120
  if (stepper_run)

121
  {

122
    if(INPUT(LS_HOME) && !stepper_dir)

123
    {

124
      if(debugging)

125
      {

126
        uart_putstr("Home erkannt\r\n");

127
        uart_putstr( itoa( step_counter, Ausgabestring, 10 ) );

128
        uart_putstr("\r\n");

129
      }

130
      stepper_home = 1;

131
      step_counter = 0;

132
      stepper_run = 0;

133
      stepper_max_min = 0;

134
      first_calc = 1;

135
    }

136
    else if(!INPUT(LS_HOME))

137
    {

138
      stepper_home = 0;

139
    }

140
141
    if(INPUT(LS_REV_HOME) && stepper_dir)

142
    {

143
      if(debugging)

144
      {

145
        uart_putstr("Rev Home erkannt\r\n");

146
        uart_putstr( itoa( step_counter, Ausgabestring, 10 ) );

147
        uart_putstr("\r\n");

148
      }

149
      stepper_rev_home = 1;

150
      stepper_run = 0;

151
      stepper_max_min = 0;

152
      first_calc = 1;

153
    }  

154
    else if(!INPUT(LS_REV_HOME))

155
    {

156
      stepper_rev_home = 0;

157
    }

158
159
    if(step_counter == position_request)

160
    {

161
      if(debugging)

162
      {

163
        uart_putstr("Counter = Request\r\n");

164
      }

165
      stepper_run = 0;

166
      stepper_max_min = 0;

167
      first_calc = 1;

168
    }

169
170
  }

171
172
/*

173
  if(debugging && stepper_command)

174
  {

175
    uart_putc( stepper_command + '0' );

176
    uart_putc( stepper_dir + '0' );

177
    uart_putc( stepper_run + '0' );

178
    uart_putc( stepper_max_min + '0' );

179
    uart_putc( stepper_home + '0' );

180
    uart_putc( stepper_rev_home + '0' );

181
    uart_putc( tick_abgelaufen + '0' );

182
    uart_putstr("\r\n");  

183
  }

184
*/

185
      

186
  switch(stepper_command)

187
  {

188
    case CMD_STEP:

189
      if(debugging)

190
        uart_putstr("STEP\r\n");

191
      

192
      stepper_off_delay = STEPPER_OFF_DELAY;

193
194
      if(stepper_dir)              // Vorwärts gehts, Richtung Top

195
      {

196
        if(!stepper_rev_home)

197
        {

198
          stepper_run = 1;

199
          position_request = step_counter + 2;

200
        }        

201
      }

202
      else                  // Zurück, Richtung Home

203
      {

204
        if(!stepper_home)

205
        {

206
          stepper_run = 1;

207
          position_request = step_counter - 2;

208
        }

209
      }

210
      break;

211
      

212
    case CMD_HOME:

213
      if(debugging)

214
        uart_putstr("HOME\r\n");

215
      if(!stepper_home)

216
      {

217
        stepper_dir = 0;

218
        stepper_run = 1;

219
        stepper_max_min = 1;      

220
        stepper_off_delay = STEPPER_OFF_DELAY;

221
        position_request = 0;

222
        ticks_rampe_akt = 0;

223
      }

224
      break;

225
      

226
    case CMD_TOP:

227
      if(debugging)

228
        uart_putstr("TOP\r\n");

229
      if(!stepper_rev_home)

230
      {

231
        stepper_dir = 1;

232
        stepper_run = 1;

233
        stepper_max_min = 1;

234
        stepper_off_delay = STEPPER_OFF_DELAY;  

235
        position_request = Stepper_max;  

236
        ticks_rampe_akt = 0;    

237
      }

238
      break;

239
      

240
    case CMD_GOTO:

241
      if(debugging)

242
        uart_putstr("GOTO\r\n");

243
      if(step_counter == position_request)

244
      {

245
        stepper_max_min = 0;

246
        stepper_run = 0;

247
      }

248
      else if(step_counter < position_request)

249
      {

250
        if(!stepper_rev_home)

251
        {

252
          stepper_dir = 1;

253
          stepper_run = 1;

254
          stepper_max_min = 1;

255
          stepper_off_delay = STEPPER_OFF_DELAY;

256
        }

257
      }

258
      else

259
      {

260
        if(!stepper_home)

261
        {

262
          stepper_dir = 0;

263
          stepper_run = 1;

264
          stepper_max_min = 1;

265
          stepper_off_delay = STEPPER_OFF_DELAY;

266
        }

267
      }

268
      ticks_rampe_akt = 0;

269
      break;

270
271
272
    case CMD_GOTO_BACKW:

273
      if(debugging)

274
        uart_putstr("GOTO_BACKW\r\n");

275
      if(position_request < step_counter)                  //Wenn x Schritte zurück > aktueller Position

276
        position_request = step_counter - position_request;        //  von der aktuellen Position aus x Schritte rückwärts (Ri. Start)

277
      else

278
      {                                  //sonst geht es nach Hause

279
        position_request = 0;

280
      }

281
      if(step_counter == position_request)                

282
      {

283
        stepper_max_min = 0;

284
        stepper_run = 0;

285
      }

286
      else if(step_counter < position_request)              

287
      {

288
        if(!stepper_rev_home)

289
        {        

290
          stepper_dir = 1;

291
          stepper_run = 1;

292
          stepper_max_min = 1;

293
          stepper_off_delay = STEPPER_OFF_DELAY;

294
        }

295
      }

296
      else

297
      {

298
        if(!stepper_home)

299
        {        

300
          stepper_dir = 0;

301
          stepper_run = 1;

302
          stepper_max_min = 1;

303
          stepper_off_delay = STEPPER_OFF_DELAY;

304
        }

305
      }

306
      ticks_rampe_akt = 0;

307
      break;

308
309
  }

310
311
  stepper_command = 0;

312
313
/*  Groß geschrieben = High-Pegel

314
315
  1  PA0  full / HALF

316
  2  PA1

317
  4  PA2

318
  8  PA3  

319
  16  PA4  

320
  32  PA5  ENABLE/disable

321
  64  PA6  CW/ccw

322
  128  PA7  Step (active low - Impuls bei steigender Flanke)

323
*/

324
325
326
  if(first_calc && stepper_run)

327
  {

328
    if(step_counter < position_request)

329
      delta = position_request - step_counter;

330
    else if(step_counter > position_request)

331
      delta = step_counter - position_request;

332
    else

333
      delta = 0;

334
      

335
    if(delta > 20)

336
    {    

337
      ticks_rampe = delta;

338
      if(debugging)

339
      {

340
        uart_putstr( itoa(ticks_rampe, Ausgabestring, 10 ) );

341
        uart_putstr(" Ticks Rampe\r\n");

342
      }

343
      

344
345
      Schritt_Faktor =        (Beschleunigung_Schritte + Bremsung_Schritte);

346
      if(Schritt_Faktor > ticks_rampe)

347
      {        

348
        Schritt_Faktor = ticks_rampe /  Schritt_Faktor;

349
      }

350
      else

351
        Schritt_Faktor = 1;

352
      if(debugging)

353
      {

354
        uart_putstr( dtostrf(Schritt_Faktor, 2, 13, Ausgabestring ) );

355
        uart_putstr(" Schrittfaktor\r\n");

356
      }

357
      

358
      ticks_rampe_beschl = Beschleunigung_Schritte * Schritt_Faktor;

359
      if(debugging)

360
      {

361
        uart_putstr( itoa(ticks_rampe_beschl, Ausgabestring, 10 ) );

362
        uart_putstr(" Ticks Rampe beschl\r\n");

363
      }

364
365
      ticks_rampe_brems  = ticks_rampe - (Bremsung_Schritte * Schritt_Faktor);      

366
      if(debugging)

367
      {

368
        uart_putstr( itoa(ticks_rampe_brems, Ausgabestring, 10 ) );

369
        uart_putstr(" Ticks Rampe brems\r\n");

370
      }

371
      

372
    }

373
374
    timer2_init(Timer_Ticks_1_int);

375
    

376
    Timer_Ticks_n    = Timer_Ticks_1;

377
    Timer_Ticks_n_int = Timer_Ticks_n;

378
    

379
    first_calc = 0;

380
  }

381
382
383
  if(tick_abgelaufen)

384
  {

385
//    if(debugging)

386
//      uart_putstr("Tick\r\n");

387
      

388
    if(stepper_run)

389
    {      

390
//      if(debugging)

391
//        uart_putstr("Tack\r\n");

392
        

393
      akt_dir = stepper_dir;  

394
  

395
//      if (stepper_off_delay == 100)

396
//      {

397
        STEPPER_PORT |= 0x20;            // Enable auf high (active) 

398
        _delay_us(1);

399
        STEPPER_PORT &= 0xBF;            // Richtung egalisieren (= Richtung HOME)

400
        _delay_us(1);

401
        if(akt_dir)

402
        {

403
          STEPPER_PORT |= 0x40;          // Richtung TOP zuweisen

404
          _delay_us(1);

405
        }

406
        STEPPER_PORT &= 0x7F;            // Clock auf low ziehen

407
        _delay_us(1);

408
        STEPPER_PORT |= 0x80;            // Steigende Flanke an Clock löst Step aus

409
        _delay_us(5);

410
        STEPPER_PORT &= 0xDF;            // Ausschalten (enable auf low)

411
//      }

412
    }

413
414
/*    

415
    if(stepper_off_delay && stepper_run)

416
    {

417
      if (stepper_max_min)

418
      {

419
        stepper_off_delay --;

420
        stepper_off_delay --;

421
        stepper_off_delay --;

422
        stepper_off_delay --;

423
        stepper_off_delay --;  

424
        stepper_off_delay --;

425
        stepper_off_delay --;

426
        stepper_off_delay --;

427
        stepper_off_delay --;

428
        stepper_off_delay --;    

429
      }

430
      else

431
      {  

432
        stepper_off_delay --;  

433
        stepper_off_delay --;

434
        stepper_off_delay --;

435
        stepper_off_delay --;

436
        stepper_off_delay --;    

437
      }      

438
    }

439
    else if(stepper_run)

440
*/    

441
         if(stepper_run)

442
    {

443
      if(stepper_max_min)

444
      {

445
//        stepper_off_delay = STEPPER_OFF_DELAY;

446
      }

447
      else

448
      {

449
        stepper_run = 0;

450
      }

451
      if(stepper_dir)

452
      {

453
        if (!stepper_rev_home)

454
        {    

455
          step_counter ++;

456
          ticks_rampe_akt ++;

457
        }          

458
      }

459
      else

460
      {

461
        if (!stepper_home)

462
        {      

463
          step_counter --;

464
          ticks_rampe_akt ++;

465
        }          

466
      }

467
    }

468
    if(stepper_run)

469
    {

470
      if(delta > 20)              //Rampe berechnen

471
      {

472
        Timer_Ticks_old = Timer_Ticks_n;

473
        

474
        if(ticks_rampe_akt <= ticks_rampe_beschl)

475
        {

476
          Q_Faktor      = Timer_Ticks_old * Timer_Ticks_old * Beschleunigung_Multiplikator;

477
          if(debugging)

478
          {

479
            uart_putstr( dtostrf(Q_Faktor, 6, 10, Ausgabestring ) );

480
//            uart_putstr(" Q Beschl\r\n");

481
          }

482
483
          Timer_Ticks_n = Timer_Ticks_old * (1 + Q_Faktor + 1.5 * Q_Faktor * Q_Faktor);

484
        }

485
        else if (ticks_rampe_akt >= ticks_rampe_brems)

486
        {

487
          Q_Faktor      = Timer_Ticks_old * Timer_Ticks_old * Bremsung_Multiplikator;

488
          if(debugging)

489
          {

490
            uart_putstr( dtostrf(Q_Faktor, 6, 10, Ausgabestring ) );

491
//            uart_putstr(" Q Brems\r\n");

492
            uart_putstr(" ; ");

493
          }

494
          

495
          Timer_Ticks_n = Timer_Ticks_old * (1 + Q_Faktor + 1.5 * Q_Faktor * Q_Faktor);          

496
        }

497
        else

498
        {

499
          Timer_Ticks_n = Timer_Ticks_old;          

500
        }

501
        

502
        if(debugging)

503
        {

504
          uart_putstr( dtostrf(Timer_Ticks_n, 6, 10, Ausgabestring ) );

505
          uart_putstr(" Akt Ticks\r\n");

506
        }

507
      

508
        Timer_Ticks_n_int = Timer_Ticks_n;

509
      }

510
      

511
      timer2_init(Timer_Ticks_n_int);

512
        

513
//      if(debugging)

514
//      {

515
//        uart_putstr( itoa(Timer_Ticks_n_int, Ausgabestring, 10 ) );

516
//        uart_putstr(" Akt Ticks int\r\n");

517
//      }

518
    }

519
  }

520
521
}

522
523
524
void stepper_init()

525
{

526
  SET_DDR(LS_LED);

527
  OUTPUT_ON(LS_LED);

528
  

529
  STEPPER_DDR   = 0xFF;

530
  STEPPER_PORT  = 0x80;                // Alles aus (Full Step bei active low), Clock high (Generell: Wechsel auf high = Schritt)

531
  

532
  position_request = Stepper_max;

533
  step_counter = Stepper_max / 2;

534
  

535
  Beschleunigung_Zeit        = (float)(VMax - VStart) / Beschleunigung;

536
  if(debugging)

537
  {

538
    uart_putstr( dtostrf( Beschleunigung_Zeit, 6, 10, Ausgabestring ) );

539
    uart_putstr(" Beschl Zeit\r\n");

540
  }

541
  

542
  Bremsung_Zeit          = (float)(VMax - VEnd  ) / Bremsung;

543
  if(debugging)

544
  {

545
    uart_putstr( dtostrf( Bremsung_Zeit, 6, 10, Ausgabestring ) );

546
    uart_putstr(" Brems Zeit\r\n");

547
  }

548
  

549
550
  Beschleunigung_Schritte  = ((uint32_t)((uint32_t)VMax * (uint32_t)VMax) - (uint32_t)((uint32_t)VStart * (uint32_t)VStart)) / (uint32_t)(2 * (uint32_t)Beschleunigung);

551
  if(debugging)

552
  {

553
    uart_putstr( ultoa( Beschleunigung_Schritte, Ausgabestring, 10 ) );

554
    uart_putstr(" Beschl Ticks\r\n");

555
  }

556
  

557
  Bremsung_Schritte    = ((uint32_t)((uint32_t)VMax * (uint32_t)VMax) - (uint32_t)((uint32_t)VEnd   * (uint32_t)VEnd  )) / (uint32_t)(2 * (uint32_t)Bremsung);

558
  if(debugging)

559
  {

560
    uart_putstr( ultoa( Bremsung_Schritte, Ausgabestring, 10 ) );

561
    uart_putstr(" Brems Ticks\r\n");

562
  }

563
564
  

565
  Frequenz            = F_CPU / Prescaler;

566
  if(debugging)

567
  {

568
    uart_putstr( ultoa( Frequenz, Ausgabestring, 10 ) );

569
    uart_putstr(" Frequenz\r\n");

570
  }

571
  

572
  Beschleunigung_Multiplikator  = (float)Beschleunigung / (Frequenz * Frequenz) * -1;;

573
  if(debugging)

574
  {

575
    uart_putstr( dtostrf( Beschleunigung_Multiplikator, 1, 14, Ausgabestring ) );

576
    uart_putstr(" Beschl Mult\r\n");

577
  }

578
  

579
  Bremsung_Multiplikator      = (float)Bremsung       / (Frequenz * Frequenz);

580
  if(debugging)

581
  {

582
    uart_putstr( dtostrf( Bremsung_Multiplikator, 1, 14, Ausgabestring ) );

583
    uart_putstr(" Brems Multr\n");  

584
  }

585
  

586
  Timer_Ticks_1                   = (float) Frequenz / sqrt(VStart * VStart + 2 * Beschleunigung);

587
  Timer_Ticks_1_int               = Timer_Ticks_1;

588
  if(debugging)

589
  {

590
    uart_putstr( dtostrf( Timer_Ticks_1, 9, 5, Ausgabestring ) );

591
    uart_putstr(" Ticks Start\r\n");

592
  }

593
}