Li Weiyi / Mbed 2 deprecated BalanceCar

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

Dependencies:   mbed

Microduino_Stepper.cpp@2:99785a1007a4, 2016-06-07 (annotated)

Committer:
lixianyu
Date:
Tue Jun 07 05:26:03 2016 +0000
Revision:
2:99785a1007a4
Parent:
0:a4d8f5b3c546 
Still not work......commit first.

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