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@7:b76e530f94f0, 2015-05-21 (annotated)

Committer:
ng3600
Date:
Thu May 21 20:17:29 2015 +0000
Revision:
7:b76e530f94f0
Parent:
6:a9ab4e153009
Child:
8:489ad954ec7b
Child:
9:15b147a67695
Tuned brakes to respond quickly

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,
ng3600 6:a9ab4e153009 100 PwmOut *motor,
ng3600 6:a9ab4e153009 101 DigitalOut *brake)
ikrase 0:18e417fff669 102 {
ikrase 0:18e417fff669 103
ikrase 0:18e417fff669 104 float delta_spd = target_spd - actual_spd;
ikrase 0:18e417fff669 105
ng3600 3:8f4f4d3a91bc 106 //if delta_spd large, brake this cycle.
ng3600 7:b76e530f94f0 107 if(delta_spd < -0.05){
ng3600 6:a9ab4e153009 108 motor->write(0.0);
ng3600 6:a9ab4e153009 109 *brake = 1;
ng3600 6:a9ab4e153009 110 return;
ng3600 6:a9ab4e153009 111 } else{
ng3600 6:a9ab4e153009 112 *brake = 0;
ng3600 6:a9ab4e153009 113 }
ng3600 3:8f4f4d3a91bc 114
ikrase 0:18e417fff669 115 (*iState) += delta_spd;
ikrase 0:18e417fff669 116 if ((*iState)*ki > iLimit) {
ikrase 0:18e417fff669 117 (*iState) = iLimit;
ikrase 0:18e417fff669 118 } else if ((*iState)*ki < -iLimit) {
ikrase 0:18e417fff669 119 (*iState) = -iLimit;
ikrase 0:18e417fff669 120 }
ikrase 0:18e417fff669 121
ikrase 0:18e417fff669 122 // PI controller!
ikrase 0:18e417fff669 123 motor->write(kp * delta_spd + ki * (*iState));
ikrase 0:18e417fff669 124 }