Li Weiyi / Mbed 2 deprecated BalanceCar

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

Dependencies:   mbed

Microduino_Stepper_PWM.cpp@3:c6caae712d5d, 2016-06-07 (annotated)

Committer:
lixianyu
Date:
Tue Jun 07 08:14:15 2016 +0000
Revision:
3:c6caae712d5d
Parent:
2:99785a1007a4 
??????????work? ?????

Who changed what in which revision?

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