Jordan Brack / SmartWheelsPublic

SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/WheelEncoder.cpp@21:60377d6f1cc2, 2017-02-14 (annotated)

Committer:
Bobymicjohn
Date:
Tue Feb 14 22:58:07 2017 +0000
Revision:
21:60377d6f1cc2
Parent:
15:eb6a274b3dfb
Child:
54:f1f5648dfacf
Fixed some bugs about the wheel encoder.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Bobymicjohn 15:eb6a274b3dfb 1 #include "WheelEncoder.h"
Bobymicjohn 15:eb6a274b3dfb 2 #include "Core.h"
Bobymicjohn 15:eb6a274b3dfb 3 #include "SWUSBServer.h"
Bobymicjohn 15:eb6a274b3dfb 4
Bobymicjohn 15:eb6a274b3dfb 5 #include "PinAssignment.h"
Bobymicjohn 15:eb6a274b3dfb 6 #include "SWUSBServer.h"
Bobymicjohn 15:eb6a274b3dfb 7
Bobymicjohn 15:eb6a274b3dfb 8 #include "mbed.h"
Bobymicjohn 15:eb6a274b3dfb 9
Bobymicjohn 15:eb6a274b3dfb 10
Bobymicjohn 15:eb6a274b3dfb 11 WheelEncoder::WheelEncoder(SW::Core& core) :
Bobymicjohn 15:eb6a274b3dfb 12 m_core(core),
Bobymicjohn 15:eb6a274b3dfb 13 m_intRL(InterruptIn(PIN_WE_RL)),
Bobymicjohn 21:60377d6f1cc2 14 m_intRR(InterruptIn(PIN_WE_RR)),
Bobymicjohn 21:60377d6f1cc2 15 m_rrDuration(1.0f),
Bobymicjohn 21:60377d6f1cc2 16 m_rlDuration(1.0f)
Bobymicjohn 15:eb6a274b3dfb 17 {
Bobymicjohn 15:eb6a274b3dfb 18 m_rrightTimer.start();
Bobymicjohn 15:eb6a274b3dfb 19 m_rleftTimer.start();
Bobymicjohn 15:eb6a274b3dfb 20 m_intRL.rise(callback(this, &WheelEncoder::pulseInRLeft));
Bobymicjohn 15:eb6a274b3dfb 21 m_intRR.rise(callback(this, &WheelEncoder::pulseInRRight));
Bobymicjohn 15:eb6a274b3dfb 22
Bobymicjohn 15:eb6a274b3dfb 23 }
Bobymicjohn 15:eb6a274b3dfb 24
Bobymicjohn 15:eb6a274b3dfb 25
Bobymicjohn 15:eb6a274b3dfb 26 void WheelEncoder::Update(float deltaTime)
Bobymicjohn 15:eb6a274b3dfb 27 {
Bobymicjohn 15:eb6a274b3dfb 28 int rrHz = 1 / m_rrDuration;
Bobymicjohn 15:eb6a274b3dfb 29 int rlHz = 1 / m_rlDuration;
Bobymicjohn 15:eb6a274b3dfb 30
Bobymicjohn 15:eb6a274b3dfb 31 char buf[5];
Bobymicjohn 15:eb6a274b3dfb 32 sprintf(buf, "RR: %d", rrHz);
Bobymicjohn 15:eb6a274b3dfb 33 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
Bobymicjohn 15:eb6a274b3dfb 34 sprintf(buf, "RL: %d", rlHz);
Bobymicjohn 15:eb6a274b3dfb 35 m_core.GetUSBServer().PushUnreliableMsg('D', buf);
Bobymicjohn 15:eb6a274b3dfb 36 }
Bobymicjohn 15:eb6a274b3dfb 37
Bobymicjohn 15:eb6a274b3dfb 38 void WheelEncoder::pulseInRRight()
Bobymicjohn 15:eb6a274b3dfb 39 {
Bobymicjohn 15:eb6a274b3dfb 40 m_rrDuration = m_rrightTimer.read();
Bobymicjohn 15:eb6a274b3dfb 41 m_rrightTimer.stop();
Bobymicjohn 15:eb6a274b3dfb 42 m_rrightTimer.reset();
Bobymicjohn 15:eb6a274b3dfb 43 m_rrightTimer.start();
Bobymicjohn 15:eb6a274b3dfb 44 }
Bobymicjohn 15:eb6a274b3dfb 45
Bobymicjohn 15:eb6a274b3dfb 46 void WheelEncoder::pulseInRLeft()
Bobymicjohn 15:eb6a274b3dfb 47 {
Bobymicjohn 15:eb6a274b3dfb 48 m_rlDuration = m_rleftTimer.read();
Bobymicjohn 15:eb6a274b3dfb 49 m_rleftTimer.stop();
Bobymicjohn 15:eb6a274b3dfb 50 m_rleftTimer.reset();
Bobymicjohn 15:eb6a274b3dfb 51 m_rleftTimer.start();
Bobymicjohn 15:eb6a274b3dfb 52 }
Bobymicjohn 15:eb6a274b3dfb 53
Bobymicjohn 15:eb6a274b3dfb 54