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@4:d5b4ba5454d4, 2015-04-27 (annotated)

Committer:
dnleek
Date:
Mon Apr 27 23:53:44 2015 +0000
Revision:
4:d5b4ba5454d4
Parent:
3:8f4f4d3a91bc
Child:
5:38487ddb0fbf
ready for param tuning

Who changed what in which revision?

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