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/CamRegBuf.cpp@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
92:e9bd429f16b5 
Better version of Intersection detection.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 30:ff7f83ad6369 1 #include "CamRegBuf.h"
hazheng 30:ff7f83ad6369 2
hazheng 30:ff7f83ad6369 3 #include "PinAssignment.h"
hazheng 30:ff7f83ad6369 4
hazheng 30:ff7f83ad6369 5 #include "CamRegDefinitions.h"
hazheng 30:ff7f83ad6369 6
hazheng 87:15fcf7891bf9 7 CamRegBuf::CamRegBuf(uint8_t writeAddr, uint8_t readAddr) :
hazheng 30:ff7f83ad6369 8 m_writeAddr(writeAddr),
hazheng 30:ff7f83ad6369 9 m_readAddr(readAddr),
hazheng 30:ff7f83ad6369 10 m_sccbCtrl(I2C(PIN_ORB_SDA, PIN_ORB_SCL))
hazheng 30:ff7f83ad6369 11 {
hazheng 30:ff7f83ad6369 12
hazheng 30:ff7f83ad6369 13 }
hazheng 30:ff7f83ad6369 14
hazheng 30:ff7f83ad6369 15 CamRegBuf::~CamRegBuf()
hazheng 30:ff7f83ad6369 16 {
hazheng 30:ff7f83ad6369 17
hazheng 30:ff7f83ad6369 18 }
hazheng 30:ff7f83ad6369 19
hazheng 30:ff7f83ad6369 20 //Blocking method. Do not use during the running state!!
hazheng 30:ff7f83ad6369 21 void CamRegBuf::SCCBWrite(uint8_t RegAddr, uint8_t Data)
hazheng 30:ff7f83ad6369 22 {
hazheng 30:ff7f83ad6369 23 m_sccbCtrl.lock();
hazheng 30:ff7f83ad6369 24 m_sccbCtrl.frequency(CAM_REG_I2CFREQ);
hazheng 30:ff7f83ad6369 25
hazheng 30:ff7f83ad6369 26 m_sccbCtrl.start();
hazheng 30:ff7f83ad6369 27
hazheng 30:ff7f83ad6369 28 m_sccbCtrl.write(m_writeAddr);
hazheng 30:ff7f83ad6369 29 wait_us(CAM_REG_WRITEWAIT);
hazheng 30:ff7f83ad6369 30 m_sccbCtrl.write(RegAddr);
hazheng 30:ff7f83ad6369 31 wait_us(CAM_REG_WRITEWAIT);
hazheng 30:ff7f83ad6369 32 m_sccbCtrl.write(Data);
hazheng 30:ff7f83ad6369 33 wait_us(CAM_REG_WRITEWAIT);
hazheng 30:ff7f83ad6369 34
hazheng 30:ff7f83ad6369 35 m_sccbCtrl.stop();
hazheng 30:ff7f83ad6369 36
hazheng 30:ff7f83ad6369 37 m_sccbCtrl.unlock();
hazheng 30:ff7f83ad6369 38 }
hazheng 30:ff7f83ad6369 39
hazheng 30:ff7f83ad6369 40 //Blocking method. Do not use during the running state!!
hazheng 30:ff7f83ad6369 41 uint8_t CamRegBuf::SCCBRead(const uint8_t RegAddr)
hazheng 30:ff7f83ad6369 42 {
hazheng 30:ff7f83ad6369 43 m_sccbCtrl.lock();
hazheng 30:ff7f83ad6369 44 m_sccbCtrl.frequency(CAM_REG_I2CFREQ);
hazheng 30:ff7f83ad6369 45
hazheng 30:ff7f83ad6369 46 m_sccbCtrl.start();
hazheng 30:ff7f83ad6369 47
hazheng 30:ff7f83ad6369 48 m_sccbCtrl.write(m_writeAddr);
hazheng 30:ff7f83ad6369 49 wait_us(CAM_REG_WRITEWAIT);
hazheng 30:ff7f83ad6369 50 m_sccbCtrl.write(RegAddr);
hazheng 32:5badeff825dc 51 wait_us(CAM_REG_WRITEWAIT);
hazheng 32:5badeff825dc 52 m_sccbCtrl.stop();
hazheng 30:ff7f83ad6369 53
hazheng 30:ff7f83ad6369 54 m_sccbCtrl.start();
hazheng 30:ff7f83ad6369 55 m_sccbCtrl.write(m_readAddr);
hazheng 30:ff7f83ad6369 56 wait_us(CAM_REG_WRITEWAIT);
hazheng 30:ff7f83ad6369 57 char readValue = m_sccbCtrl.read(CAM_REG_NOACK);
hazheng 30:ff7f83ad6369 58
hazheng 30:ff7f83ad6369 59 m_sccbCtrl.stop();
hazheng 30:ff7f83ad6369 60
hazheng 30:ff7f83ad6369 61 m_sccbCtrl.unlock();
hazheng 30:ff7f83ad6369 62
hazheng 30:ff7f83ad6369 63 return readValue;
hazheng 30:ff7f83ad6369 64 }
hazheng 30:ff7f83ad6369 65
hazheng 92:e9bd429f16b5 66 /*
hazheng 30:ff7f83ad6369 67 //Blocking method. Do not use during the running state!!
hazheng 30:ff7f83ad6369 68 void CamRegBuf::ReadRegisters()
hazheng 30:ff7f83ad6369 69 {
hazheng 30:ff7f83ad6369 70 for(int i = 0; i <= 0xff; ++i)
hazheng 30:ff7f83ad6369 71 {
Bobymicjohn 54:f1f5648dfacf 72 //char buf[10];
Bobymicjohn 54:f1f5648dfacf 73 //sprintf(buf, "%#x-%#x", i, SCCBRead(i));
Bobymicjohn 54:f1f5648dfacf 74 //m_core.GetUSBServer().PushUnreliableMsg('D', buf);
hazheng 30:ff7f83ad6369 75
hazheng 30:ff7f83ad6369 76 wait(0.1);
hazheng 30:ff7f83ad6369 77 }
hazheng 30:ff7f83ad6369 78 }
hazheng 30:ff7f83ad6369 79
hazheng 32:5badeff825dc 80 int32_t CamRegBuf::I2CBufferRead(int32_t ucDevAddr, uint8_t *ucBuffer, int32_t ulSize)
hazheng 32:5badeff825dc 81 {
hazheng 32:5badeff825dc 82 m_sccbCtrl.start();
hazheng 32:5badeff825dc 83
hazheng 32:5badeff825dc 84 m_sccbCtrl.write(ucDevAddr);
hazheng 32:5badeff825dc 85 if(ulSize == 1){
hazheng 32:5badeff825dc 86 ucBuffer[0] = m_sccbCtrl.read(0);
hazheng 32:5badeff825dc 87 }else{
hazheng 32:5badeff825dc 88 for(int i=0;i<ulSize;i++){
hazheng 32:5badeff825dc 89 ucBuffer[i] = m_sccbCtrl.read(0);
hazheng 32:5badeff825dc 90 }
hazheng 32:5badeff825dc 91 }
hazheng 32:5badeff825dc 92
hazheng 32:5badeff825dc 93 m_sccbCtrl.stop();
hazheng 32:5badeff825dc 94
hazheng 32:5badeff825dc 95 return 0;
hazheng 32:5badeff825dc 96 }
hazheng 32:5badeff825dc 97
hazheng 32:5badeff825dc 98 int32_t CamRegBuf::I2CBufferWrite(int32_t ucDevAddr, uint8_t *ucBuffer, int32_t ulSize)
hazheng 32:5badeff825dc 99 {
hazheng 32:5badeff825dc 100 m_sccbCtrl.start();
hazheng 32:5badeff825dc 101
hazheng 32:5badeff825dc 102 // Set I2C slave write address
hazheng 32:5badeff825dc 103 m_sccbCtrl.write(ucDevAddr);
hazheng 32:5badeff825dc 104
hazheng 32:5badeff825dc 105 if(ulSize == 1){
hazheng 32:5badeff825dc 106 m_sccbCtrl.write(ucBuffer[0]);
hazheng 32:5badeff825dc 107 }else{
hazheng 32:5badeff825dc 108 for(int i=0;i<ulSize;i++){
hazheng 32:5badeff825dc 109 m_sccbCtrl.write(ucBuffer[i]);
hazheng 32:5badeff825dc 110 }
hazheng 32:5badeff825dc 111 }
hazheng 32:5badeff825dc 112
hazheng 32:5badeff825dc 113 m_sccbCtrl.stop();
hazheng 32:5badeff825dc 114
hazheng 32:5badeff825dc 115 return 0;
hazheng 32:5badeff825dc 116 }
hazheng 32:5badeff825dc 117 */
hazheng 30:ff7f83ad6369 118 void CamRegBuf::WriteRegSet(const struct sensor_reg * reglist)
hazheng 30:ff7f83ad6369 119 {
hazheng 30:ff7f83ad6369 120 unsigned int i = 0;
hazheng 30:ff7f83ad6369 121 while ((reglist[i].reg != 0xff) | (reglist[i].val != 0xff))
hazheng 30:ff7f83ad6369 122 {
hazheng 30:ff7f83ad6369 123 SCCBWrite(reglist[i].reg, reglist[i].val);
hazheng 30:ff7f83ad6369 124 ++i;
hazheng 32:5badeff825dc 125 wait_ms(1);
hazheng 30:ff7f83ad6369 126 }
hazheng 30:ff7f83ad6369 127 }