Li Weiyi / Mbed 2 deprecated BalanceCar

虽然移植完毕，但是不work。需要细调……

Dependencies:   mbed

Microduino_Stepper.cpp@0:a4d8f5b3c546, 2016-06-04 (annotated)

Committer:
lixianyu
Date:
Sat Jun 04 03:16:52 2016 +0000
Revision:
0:a4d8f5b3c546
Child:
2:99785a1007a4
Pass compile!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lixianyu 0:a4d8f5b3c546 1 // Microduino_Stepper.cpp
lixianyu 0:a4d8f5b3c546 2 //
lixianyu 0:a4d8f5b3c546 3 // Copyright (C) 2009-2013 Shenyang
lixianyu 0:a4d8f5b3c546 4 // $Id: Microduino_Stepper.cpp,v 1.00 2016/04/07 $
lixianyu 0:a4d8f5b3c546 5
lixianyu 0:a4d8f5b3c546 6 #include "Microduino_Stepper.h"
lixianyu 0:a4d8f5b3c546 7 extern Timer g_Timer;
lixianyu 0:a4d8f5b3c546 8 DigitalOut gSteperE(PIN_EN);
lixianyu 0:a4d8f5b3c546 9 Ticker g_Ticker;
lixianyu 0:a4d8f5b3c546 10 static Stepper_t steppers[MAX_STEPPERS];
lixianyu 0:a4d8f5b3c546 11
lixianyu 0:a4d8f5b3c546 12 uint8_t StepperCount = 0;
lixianyu 0:a4d8f5b3c546 13
lixianyu 0:a4d8f5b3c546 14 // Some debugging assistance
lixianyu 0:a4d8f5b3c546 15
lixianyu 0:a4d8f5b3c546 16 void stepperAllEnable()
lixianyu 0:a4d8f5b3c546 17 {
lixianyu 0:a4d8f5b3c546 18 //digitalWrite(PIN_EN, LOW);
lixianyu 0:a4d8f5b3c546 19 gSteperE = 0;
lixianyu 0:a4d8f5b3c546 20 }
lixianyu 0:a4d8f5b3c546 21
lixianyu 0:a4d8f5b3c546 22 void stepperAllDisable()
lixianyu 0:a4d8f5b3c546 23 {
lixianyu 0:a4d8f5b3c546 24 //digitalWrite(PIN_EN, HIGH);
lixianyu 0:a4d8f5b3c546 25 gSteperE = 1;
lixianyu 0:a4d8f5b3c546 26 }
lixianyu 0:a4d8f5b3c546 27
lixianyu 0:a4d8f5b3c546 28 static inline void handle_interrupts()
lixianyu 0:a4d8f5b3c546 29 {
lixianyu 0:a4d8f5b3c546 30 for(uint8_t channel=0; channel < MAX_STEPPERS; channel++) {
lixianyu 0:a4d8f5b3c546 31 if(steppers[channel].isActive)
lixianyu 0:a4d8f5b3c546 32 steppers[channel].stepper->computeStep();
lixianyu 0:a4d8f5b3c546 33 }
lixianyu 0:a4d8f5b3c546 34 }
lixianyu 0:a4d8f5b3c546 35
lixianyu 0:a4d8f5b3c546 36 #if 0
lixianyu 0:a4d8f5b3c546 37 static void initISR()
lixianyu 0:a4d8f5b3c546 38 {
lixianyu 0:a4d8f5b3c546 39 #if defined(_useTimer1)
lixianyu 0:a4d8f5b3c546 40 cli();
lixianyu 0:a4d8f5b3c546 41 TCCR1A = 0;
lixianyu 0:a4d8f5b3c546 42 TCCR1B = _BV(WGM12)|_BV(CS11);
lixianyu 0:a4d8f5b3c546 43 OCR1A = TIMER_COMP;
lixianyu 0:a4d8f5b3c546 44 TCNT1 = 0;
lixianyu 0:a4d8f5b3c546 45 TIMSK1 = _BV(OCIE1A);
lixianyu 0:a4d8f5b3c546 46 sei();
lixianyu 0:a4d8f5b3c546 47 #endif
lixianyu 0:a4d8f5b3c546 48 pinMode(PIN_EN, OUTPUT);
lixianyu 0:a4d8f5b3c546 49 stepperAllEnable();
lixianyu 0:a4d8f5b3c546 50 }
lixianyu 0:a4d8f5b3c546 51
lixianyu 0:a4d8f5b3c546 52 #if defined(_useTimer1)
lixianyu 0:a4d8f5b3c546 53 ISR(TIMER1_COMPA_vect)
lixianyu 0:a4d8f5b3c546 54 {
lixianyu 0:a4d8f5b3c546 55 handle_interrupts();
lixianyu 0:a4d8f5b3c546 56 }
lixianyu 0:a4d8f5b3c546 57 #endif
lixianyu 0:a4d8f5b3c546 58 #endif
lixianyu 0:a4d8f5b3c546 59
lixianyu 0:a4d8f5b3c546 60 static void timerHandle()
lixianyu 0:a4d8f5b3c546 61 {
lixianyu 0:a4d8f5b3c546 62 for (uint8_t channel=0; channel < MAX_STEPPERS; channel++) {
lixianyu 0:a4d8f5b3c546 63 if(steppers[channel].isActive)
lixianyu 0:a4d8f5b3c546 64 steppers[channel].stepper->computeStep();
lixianyu 0:a4d8f5b3c546 65 }
lixianyu 0:a4d8f5b3c546 66 }
lixianyu 0:a4d8f5b3c546 67
lixianyu 0:a4d8f5b3c546 68 static bool isTimerActive()
lixianyu 0:a4d8f5b3c546 69 {
lixianyu 0:a4d8f5b3c546 70 for(uint8_t channel=0; channel <MAX_STEPPERS ; channel++) {
lixianyu 0:a4d8f5b3c546 71 if(steppers[channel].isActive == true)
lixianyu 0:a4d8f5b3c546 72 return true;
lixianyu 0:a4d8f5b3c546 73 }
lixianyu 0:a4d8f5b3c546 74 return false;
lixianyu 0:a4d8f5b3c546 75 }
lixianyu 0:a4d8f5b3c546 76 /****************** end of static functions ******************************/
lixianyu 0:a4d8f5b3c546 77
lixianyu 0:a4d8f5b3c546 78 Stepper::Stepper(uint8_t _dirPin, uint8_t _stepPin)
lixianyu 0:a4d8f5b3c546 79 {
lixianyu 0:a4d8f5b3c546 80 if(StepperCount < MAX_STEPPERS) {
lixianyu 0:a4d8f5b3c546 81 this->stepperIndex = StepperCount++;
lixianyu 0:a4d8f5b3c546 82 } else {
lixianyu 0:a4d8f5b3c546 83 this->stepperIndex = INVALID_STEPPER;
lixianyu 0:a4d8f5b3c546 84 }
lixianyu 0:a4d8f5b3c546 85 dirPin = _dirPin;
lixianyu 0:a4d8f5b3c546 86 stepPin = _stepPin;
lixianyu 0:a4d8f5b3c546 87 speed = 0;
lixianyu 0:a4d8f5b3c546 88 period = 0;
lixianyu 0:a4d8f5b3c546 89 counter = 0;
lixianyu 0:a4d8f5b3c546 90 steppers[this->stepperIndex].isActive = false;
lixianyu 0:a4d8f5b3c546 91 }
lixianyu 0:a4d8f5b3c546 92
lixianyu 0:a4d8f5b3c546 93 uint8_t Stepper::begin()
lixianyu 0:a4d8f5b3c546 94 {
lixianyu 0:a4d8f5b3c546 95 if (this->stepperIndex < MAX_STEPPERS) {
lixianyu 0:a4d8f5b3c546 96 #if 0
lixianyu 0:a4d8f5b3c546 97 pinMode(dirPin, OUTPUT);
lixianyu 0:a4d8f5b3c546 98 pinMode(stepPin, OUTPUT);
lixianyu 0:a4d8f5b3c546 99 #else
lixianyu 0:a4d8f5b3c546 100 gpio_init_out(&dirOUT, (PinName)dirPin);
lixianyu 0:a4d8f5b3c546 101 gpio_init_out(&stepOUT, (PinName)stepPin);
lixianyu 0:a4d8f5b3c546 102 #endif
lixianyu 0:a4d8f5b3c546 103 setMaxAccel(DEFAULT_ACCEL);
lixianyu 0:a4d8f5b3c546 104 if (isTimerActive() == false) {
lixianyu 0:a4d8f5b3c546 105 //initISR();
lixianyu 0:a4d8f5b3c546 106 //pinMode(PIN_EN, OUTPUT);
lixianyu 0:a4d8f5b3c546 107 g_Ticker.attach_us(&timerHandle, 25);
lixianyu 0:a4d8f5b3c546 108 stepperAllEnable();
lixianyu 0:a4d8f5b3c546 109 }
lixianyu 0:a4d8f5b3c546 110 steppers[this->stepperIndex].isActive = true;
lixianyu 0:a4d8f5b3c546 111 steppers[this->stepperIndex].stepper = this;
lixianyu 0:a4d8f5b3c546 112 }
lixianyu 0:a4d8f5b3c546 113 return this->stepperIndex;
lixianyu 0:a4d8f5b3c546 114 }
lixianyu 0:a4d8f5b3c546 115 #define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
lixianyu 0:a4d8f5b3c546 116 void Stepper::setSpeed(int16_t _speed)
lixianyu 0:a4d8f5b3c546 117 {
lixianyu 0:a4d8f5b3c546 118 speed += constrain((_speed-speed), -(int16_t)maxAccel, (int16_t)maxAccel);
lixianyu 0:a4d8f5b3c546 119 if (speed == 0) {
lixianyu 0:a4d8f5b3c546 120 period = 0;
lixianyu 0:a4d8f5b3c546 121 } else {
lixianyu 0:a4d8f5b3c546 122 period = MAX_SPEED_S / abs(speed);
lixianyu 0:a4d8f5b3c546 123 }
lixianyu 0:a4d8f5b3c546 124 //(speed>0) ? PIN_CLR(dirPin) : PIN_SET(dirPin);
lixianyu 0:a4d8f5b3c546 125 if (speed > 0) {
lixianyu 0:a4d8f5b3c546 126 gpio_write(&dirOUT, 0);
lixianyu 0:a4d8f5b3c546 127 } else {
lixianyu 0:a4d8f5b3c546 128 gpio_write(&dirOUT, 1);
lixianyu 0:a4d8f5b3c546 129 }
lixianyu 0:a4d8f5b3c546 130 }
lixianyu 0:a4d8f5b3c546 131
lixianyu 0:a4d8f5b3c546 132 void Stepper::setMaxAccel(uint16_t _accel)
lixianyu 0:a4d8f5b3c546 133 {
lixianyu 0:a4d8f5b3c546 134 maxAccel = _accel;
lixianyu 0:a4d8f5b3c546 135 }
lixianyu 0:a4d8f5b3c546 136
lixianyu 0:a4d8f5b3c546 137 int16_t Stepper::getSpeed()
lixianyu 0:a4d8f5b3c546 138 {
lixianyu 0:a4d8f5b3c546 139 return speed;
lixianyu 0:a4d8f5b3c546 140 }
lixianyu 0:a4d8f5b3c546 141
lixianyu 0:a4d8f5b3c546 142 uint16_t Stepper::getMaxAccel()
lixianyu 0:a4d8f5b3c546 143 {
lixianyu 0:a4d8f5b3c546 144 return maxAccel;
lixianyu 0:a4d8f5b3c546 145 }
lixianyu 0:a4d8f5b3c546 146
lixianyu 0:a4d8f5b3c546 147 void Stepper::computeStep()
lixianyu 0:a4d8f5b3c546 148 {
lixianyu 0:a4d8f5b3c546 149 counter++;
lixianyu 0:a4d8f5b3c546 150 if(counter > period) {
lixianyu 0:a4d8f5b3c546 151 counter = 0;
lixianyu 0:a4d8f5b3c546 152 if(period > 0) {
lixianyu 0:a4d8f5b3c546 153 //PIN_SET(stepPin);
lixianyu 0:a4d8f5b3c546 154 gpio_write(&stepOUT, 1);
lixianyu 0:a4d8f5b3c546 155 //delayMicroseconds(1);
lixianyu 0:a4d8f5b3c546 156 wait_us(1);
lixianyu 0:a4d8f5b3c546 157 //PIN_CLR(stepPin);
lixianyu 0:a4d8f5b3c546 158 gpio_write(&stepOUT, 0);
lixianyu 0:a4d8f5b3c546 159 }
lixianyu 0:a4d8f5b3c546 160 }
lixianyu 0:a4d8f5b3c546 161 }