Team 9 / Motor

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Motor.cpp@3:8f4f4d3a91bc, 2015-04-25 (annotated)

Committer:
ng3600
Date:
Sat Apr 25 02:42:23 2015 +0000
Revision:
3:8f4f4d3a91bc
Parent:
2:3bf51023ea23
Child:
4:d5b4ba5454d4
Child:
6:a9ab4e153009
not sure what changed here

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ikrase 0:18e417fff669 1 #include "Motor.h"
ikrase 0:18e417fff669 2
ikrase 0:18e417fff669 3 /* Theory of operation of the tachometer reading system
ikrase 0:18e417fff669 4
ikrase 0:18e417fff669 5 The interrupt is Tach, an InterruptIn object.
ikrase 0:18e417fff669 6
ikrase 0:18e417fff669 7 To filter out the horrible transients, some debounce and stuff is being done. This function, tach_mark(), is called
ikrase 0:18e417fff669 8 on a rising edge. It checks whether the risen edge is still there 10us later. (Using a blocking wait_us() call,
ikrase 0:18e417fff669 9 which is bad practice, but not that bad considering how short 10 us is. I don't think it would be that difficult to instead have it \
ikrase 0:18e417fff669 10 set up a 10us delay and nonblockingly interrupt-call a function using a Timeout.
ikrase 0:18e417fff669 11
ikrase 0:18e417fff669 12 Anyway, if it is still there, it then records the read from TachTimer and resets the TachTimer, and adds the read value to
ikrase 0:18e417fff669 13 the ring buffer and to the delta_t global.
ikrase 0:18e417fff669 14
ikrase 0:18e417fff669 15 It *also* disables the Tach rising edge interrupt. (i.e disables itself) and enables a ~~falling~~ edge interrupt on Tach. This
ikrase 0:18e417fff669 16 falling edge interupt then does mostly the same thing w/r/t filtering, and enables the rising edge after a confirmed falling edge.
ikrase 0:18e417fff669 17
ikrase 0:18e417fff669 18 */
ikrase 0:18e417fff669 19 void addToRingBuf(int val, volatile int *ringBuf, volatile int *ringBufIdx) //Is called in ISR. Adds stuff to ring buffer, obviously.
ikrase 0:18e417fff669 20 {
ikrase 0:18e417fff669 21 ringBuf[*ringBufIdx] = val;
ng3600 2:3bf51023ea23 22 *ringBufIdx = (*ringBufIdx + 1) % RING_BUF_SZ; //avoid mod, do if
ikrase 0:18e417fff669 23 }
ikrase 0:18e417fff669 24
ikrase 0:18e417fff669 25
ikrase 0:18e417fff669 26 void tach_mark(volatile int *ringBuf,
ikrase 0:18e417fff669 27 volatile int *ringBufIdx,
ikrase 0:18e417fff669 28 InterruptIn *Tach,
ikrase 0:18e417fff669 29 Timer *TachTimer,
ikrase 0:18e417fff669 30 volatile bool *new_tach_flag,
ikrase 0:18e417fff669 31 void (*tach_antimark_handler)(void)) //PRIMARY ISR called on rising edge of encoder.
ikrase 0:18e417fff669 32 {
ikrase 0:18e417fff669 33 int dt = TachTimer->read_us();
ikrase 0:18e417fff669 34
ikrase 0:18e417fff669 35 wait_us(10);
ikrase 0:18e417fff669 36 if(Tach->read()) {
ikrase 0:18e417fff669 37 TachTimer->reset();
ikrase 0:18e417fff669 38 *new_tach_flag = 1;
ikrase 0:18e417fff669 39 addToRingBuf(dt, ringBuf, ringBufIdx);
ikrase 0:18e417fff669 40 Tach->rise(NULL);
ikrase 0:18e417fff669 41 Tach->fall(tach_antimark_handler); // Reset after a falling edge. //set to handler for antimark
ikrase 0:18e417fff669 42 }
ikrase 0:18e417fff669 43 }
ikrase 0:18e417fff669 44
ikrase 0:18e417fff669 45
ikrase 0:18e417fff669 46 void tach_antimark(InterruptIn *Tach, void (*tach_mark_handler)(void))
ikrase 0:18e417fff669 47 {
ikrase 0:18e417fff669 48 wait_us(10); // This can surely be done better with a Timeout. Medium priority. Highest is getting it in a
ikrase 0:18e417fff669 49 //library/object and to work with two encoders at once.
ikrase 0:18e417fff669 50 if(!Tach->read()) {
ikrase 0:18e417fff669 51 Tach->fall(NULL);
ikrase 0:18e417fff669 52 Tach->rise(tach_mark_handler); //set to handler function for mark
ikrase 0:18e417fff669 53 }
ikrase 0:18e417fff669 54 }
ikrase 0:18e417fff669 55
ikrase 0:18e417fff669 56
ikrase 0:18e417fff669 57 float get_speed(volatile bool *new_tach_flag,
ikrase 0:18e417fff669 58 Timer *TachTimer,
ikrase 0:18e417fff669 59 volatile int *ringBuf,
ikrase 0:18e417fff669 60 volatile int *ringBufIdx)
ikrase 0:18e417fff669 61 {
ikrase 0:18e417fff669 62 if(new_tach_flag) {
ikrase 0:18e417fff669 63 new_tach_flag = 0;
ikrase 0:18e417fff669 64 }
ikrase 0:18e417fff669 65
ikrase 0:18e417fff669 66 // if no pulse in 400ms, motor is probably stopped
ikrase 0:18e417fff669 67 // 3% duty cycle is lowest the motor will run with, has <400ms period
ikrase 0:18e417fff669 68 if(TachTimer->read_ms() > 400) {
ikrase 0:18e417fff669 69 addToRingBuf(INT_MAX, ringBuf, ringBufIdx);
ikrase 0:18e417fff669 70 TachTimer->reset();
ikrase 0:18e417fff669 71 }
ikrase 0:18e417fff669 72
ikrase 0:18e417fff669 73 int avg = 0;
ikrase 0:18e417fff669 74 for (int i = 0; i < RING_BUF_SZ; i++) {
ikrase 0:18e417fff669 75 if (ringBuf[i] == INT_MAX) {
ikrase 0:18e417fff669 76 avg = INT_MAX;
ikrase 0:18e417fff669 77 break;
ikrase 0:18e417fff669 78 }
ikrase 0:18e417fff669 79 avg += ringBuf[i];
ikrase 0:18e417fff669 80 }
ikrase 0:18e417fff669 81
ikrase 0:18e417fff669 82 if (avg == INT_MAX) {
ikrase 0:18e417fff669 83 return 0;
ikrase 0:18e417fff669 84 } else {
ikrase 0:18e417fff669 85 avg = avg / RING_BUF_SZ;
ikrase 0:18e417fff669 86 if (avg > 400000) { //400 ms
ikrase 0:18e417fff669 87 return 0;
ikrase 0:18e417fff669 88 } else {
ikrase 0:18e417fff669 89 return WHEEL_CIRCUM / ((2.0 * (float)avg) / 1000.0);
ikrase 0:18e417fff669 90 }
ikrase 0:18e417fff669 91 }
ikrase 0:18e417fff669 92 }
ikrase 0:18e417fff669 93
ikrase 0:18e417fff669 94 void speed_control(volatile float target_spd,
ikrase 0:18e417fff669 95 float actual_spd,
ikrase 0:18e417fff669 96 float *iState,
ikrase 0:18e417fff669 97 float iLimit,
ikrase 0:18e417fff669 98 volatile float ki,
ikrase 0:18e417fff669 99 volatile float kp,
ikrase 0:18e417fff669 100 PwmOut *motor)
ikrase 0:18e417fff669 101 {
ikrase 0:18e417fff669 102
ikrase 0:18e417fff669 103 float delta_spd = target_spd - actual_spd;
ikrase 0:18e417fff669 104
ng3600 3:8f4f4d3a91bc 105 //if delta_spd large, brake this cycle.
ng3600 3:8f4f4d3a91bc 106
ikrase 0:18e417fff669 107 (*iState) += delta_spd;
ikrase 0:18e417fff669 108 if ((*iState)*ki > iLimit) {
ikrase 0:18e417fff669 109 (*iState) = iLimit;
ikrase 0:18e417fff669 110 } else if ((*iState)*ki < -iLimit) {
ikrase 0:18e417fff669 111 (*iState) = -iLimit;
ikrase 0:18e417fff669 112 }
ikrase 0:18e417fff669 113
ikrase 0:18e417fff669 114 // PI controller!
ikrase 0:18e417fff669 115 motor->write(kp * delta_spd + ki * (*iState));
ikrase 0:18e417fff669 116 }