CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

lib/adns_9800/adns_9800.cpp@38:67bc78f3c0ab, 2016-03-27 (annotated)

Committer:
lilac0112_1
Date:
Sun Mar 27 13:04:39 2016 +0000
Revision:
38:67bc78f3c0ab
Parent:
0:ea35c18c85fc 
JapanSoccerOpen2016 CatPot Program(main)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 /**
lilac0112_1 0:ea35c18c85fc 2 * @author Alexander Entinger, MSc / LXRobotics
lilac0112_1 0:ea35c18c85fc 3 * @brief this class acts as interface for accessing the adns-9800 sensor - based on https://github.com/mrjohnk/ADNS-9800
lilac0112_1 0:ea35c18c85fc 4 * @file adns_9800.cpp
lilac0112_1 0:ea35c18c85fc 5 */
lilac0112_1 0:ea35c18c85fc 6
lilac0112_1 0:ea35c18c85fc 7 #include "adns_9800.h"
lilac0112_1 0:ea35c18c85fc 8 #include "adns_9800_regs.h"
lilac0112_1 0:ea35c18c85fc 9 #include "adns_9800_srom_a4.h"
lilac0112_1 0:ea35c18c85fc 10
lilac0112_1 0:ea35c18c85fc 11 /**
lilac0112_1 0:ea35c18c85fc 12 * @brief Constructor
lilac0112_1 0:ea35c18c85fc 13 */
lilac0112_1 0:ea35c18c85fc 14 adns_9800::adns_9800(PinName mosi, PinName miso, PinName sclk, PinName ncs) : m_spi(mosi, miso, sclk), m_ncs_pin(ncs)
lilac0112_1 0:ea35c18c85fc 15 {
lilac0112_1 0:ea35c18c85fc 16 m_spi.format(8, 3); // 8 bits with mode 3 =>Polarity = 1, Phase = 1
lilac0112_1 0:ea35c18c85fc 17 m_spi.frequency(1000000); // 1 MHz
lilac0112_1 0:ea35c18c85fc 18
lilac0112_1 0:ea35c18c85fc 19 startup();
lilac0112_1 0:ea35c18c85fc 20 }
lilac0112_1 0:ea35c18c85fc 21
lilac0112_1 0:ea35c18c85fc 22 /**
lilac0112_1 0:ea35c18c85fc 23 * @brief Destructor
lilac0112_1 0:ea35c18c85fc 24 */
lilac0112_1 0:ea35c18c85fc 25 adns_9800::~adns_9800()
lilac0112_1 0:ea35c18c85fc 26 {
lilac0112_1 0:ea35c18c85fc 27
lilac0112_1 0:ea35c18c85fc 28 }
lilac0112_1 0:ea35c18c85fc 29
lilac0112_1 0:ea35c18c85fc 30 /**
lilac0112_1 0:ea35c18c85fc 31 * @brief returns true if a motion has occured since the last readout
lilac0112_1 0:ea35c18c85fc 32 */
lilac0112_1 0:ea35c18c85fc 33 bool adns_9800::new_motion_data_available()
lilac0112_1 0:ea35c18c85fc 34 {
lilac0112_1 0:ea35c18c85fc 35 uint8_t const motion_reg = read_reg(REG_Motion);
lilac0112_1 0:ea35c18c85fc 36 bool const new_data_available = (motion_reg & 0x80) > 0;
lilac0112_1 0:ea35c18c85fc 37 return new_data_available;
lilac0112_1 0:ea35c18c85fc 38 }
lilac0112_1 0:ea35c18c85fc 39
lilac0112_1 0:ea35c18c85fc 40 /**
lilac0112_1 0:ea35c18c85fc 41 * @brief retrieves the latest delta values
lilac0112_1 0:ea35c18c85fc 42 */
lilac0112_1 0:ea35c18c85fc 43 void adns_9800::get_delta_x_y(int16_t &delta_x, int16_t &delta_y)
lilac0112_1 0:ea35c18c85fc 44 {
lilac0112_1 0:ea35c18c85fc 45 uint16_t delta_x_l = (uint16_t)(read_reg(REG_Delta_X_L));
lilac0112_1 0:ea35c18c85fc 46 uint16_t delta_x_h = (uint16_t)(read_reg(REG_Delta_X_H)) << 8;
lilac0112_1 0:ea35c18c85fc 47 delta_x = (int16_t)(delta_x_h | delta_x_l);
lilac0112_1 0:ea35c18c85fc 48
lilac0112_1 0:ea35c18c85fc 49 uint16_t delta_y_l = (uint16_t)(read_reg(REG_Delta_Y_L));
lilac0112_1 0:ea35c18c85fc 50 uint16_t delta_y_h = (uint16_t)(read_reg(REG_Delta_Y_H)) << 8;
lilac0112_1 0:ea35c18c85fc 51 delta_y = (int16_t)(delta_y_h | delta_y_l);
lilac0112_1 0:ea35c18c85fc 52 }
lilac0112_1 0:ea35c18c85fc 53
lilac0112_1 0:ea35c18c85fc 54 /**
lilac0112_1 0:ea35c18c85fc 55 * @brief start and stop communication with the sensor by clearing/setting the ncs pin
lilac0112_1 0:ea35c18c85fc 56 */
lilac0112_1 0:ea35c18c85fc 57 void adns_9800::com_begin()
lilac0112_1 0:ea35c18c85fc 58 {
lilac0112_1 0:ea35c18c85fc 59 m_ncs_pin = 0;
lilac0112_1 0:ea35c18c85fc 60 }
lilac0112_1 0:ea35c18c85fc 61 void adns_9800::com_end()
lilac0112_1 0:ea35c18c85fc 62 {
lilac0112_1 0:ea35c18c85fc 63 m_ncs_pin = 1;
lilac0112_1 0:ea35c18c85fc 64 }
lilac0112_1 0:ea35c18c85fc 65
lilac0112_1 0:ea35c18c85fc 66 /**
lilac0112_1 0:ea35c18c85fc 67 * @brief provide read/write access to a adns register
lilac0112_1 0:ea35c18c85fc 68 */
lilac0112_1 0:ea35c18c85fc 69 uint8_t adns_9800::read_reg(uint8_t const address)
lilac0112_1 0:ea35c18c85fc 70 {
lilac0112_1 0:ea35c18c85fc 71 com_begin();
lilac0112_1 0:ea35c18c85fc 72
lilac0112_1 0:ea35c18c85fc 73 // send adress of the register, with MSBit = 0 to indicate it's a read
lilac0112_1 0:ea35c18c85fc 74 m_spi.write(address & 0x7f );
lilac0112_1 0:ea35c18c85fc 75 wait_us(100); // tSRAD
lilac0112_1 0:ea35c18c85fc 76 // read data
lilac0112_1 0:ea35c18c85fc 77 uint8_t data = m_spi.write(0);
lilac0112_1 0:ea35c18c85fc 78
lilac0112_1 0:ea35c18c85fc 79 wait_us(1); // tSCLK-NCS for read operation is 120ns
lilac0112_1 0:ea35c18c85fc 80 com_end();
lilac0112_1 0:ea35c18c85fc 81 wait_us(19); // tSRW/tSRR (=20us) minus tSCLK-NCS
lilac0112_1 0:ea35c18c85fc 82
lilac0112_1 0:ea35c18c85fc 83 return data;
lilac0112_1 0:ea35c18c85fc 84 }
lilac0112_1 0:ea35c18c85fc 85 void adns_9800::write_reg(uint8_t const address, uint8_t const data)
lilac0112_1 0:ea35c18c85fc 86 {
lilac0112_1 0:ea35c18c85fc 87 com_begin();
lilac0112_1 0:ea35c18c85fc 88
lilac0112_1 0:ea35c18c85fc 89 //send adress of the register, with MSBit = 1 to indicate it's a write
lilac0112_1 0:ea35c18c85fc 90 m_spi.write(address | 0x80 );
lilac0112_1 0:ea35c18c85fc 91 //sent data
lilac0112_1 0:ea35c18c85fc 92 m_spi.write(data);
lilac0112_1 0:ea35c18c85fc 93
lilac0112_1 0:ea35c18c85fc 94 wait_us(20); // tSCLK-NCS for write operation
lilac0112_1 0:ea35c18c85fc 95 com_end();
lilac0112_1 0:ea35c18c85fc 96 wait_us(100); // tSWW/tSWR (=120us) minus tSCLK-NCS. Could be shortened, but is looks like a safe lower bound
lilac0112_1 0:ea35c18c85fc 97 }
lilac0112_1 0:ea35c18c85fc 98
lilac0112_1 0:ea35c18c85fc 99 /**
lilac0112_1 0:ea35c18c85fc 100 * @brief upload the firmware
lilac0112_1 0:ea35c18c85fc 101 */
lilac0112_1 0:ea35c18c85fc 102 void adns_9800::upload_firmware()
lilac0112_1 0:ea35c18c85fc 103 {
lilac0112_1 0:ea35c18c85fc 104 // set the configuration_IV register in 3k firmware mode
lilac0112_1 0:ea35c18c85fc 105 write_reg(REG_Configuration_IV, 0x02); // bit 1 = 1 for 3k mode, other bits are reserved
lilac0112_1 0:ea35c18c85fc 106
lilac0112_1 0:ea35c18c85fc 107 // write 0x1d in SROM_enable reg for initializing
lilac0112_1 0:ea35c18c85fc 108 write_reg(REG_SROM_Enable, 0x1d);
lilac0112_1 0:ea35c18c85fc 109
lilac0112_1 0:ea35c18c85fc 110 // wait for more than one frame period
lilac0112_1 0:ea35c18c85fc 111 wait_ms(10); // assume that the frame rate is as low as 100fps... even if it should never be that low
lilac0112_1 0:ea35c18c85fc 112
lilac0112_1 0:ea35c18c85fc 113 // write 0x18 to SROM_enable to start SROM download
lilac0112_1 0:ea35c18c85fc 114 write_reg(REG_SROM_Enable, 0x18);
lilac0112_1 0:ea35c18c85fc 115
lilac0112_1 0:ea35c18c85fc 116 // write the SROM file (=firmware data)
lilac0112_1 0:ea35c18c85fc 117 com_begin();
lilac0112_1 0:ea35c18c85fc 118 m_spi.write(REG_SROM_Load_Burst | 0x80); // write burst destination adress
lilac0112_1 0:ea35c18c85fc 119 wait_us(15);
lilac0112_1 0:ea35c18c85fc 120
lilac0112_1 0:ea35c18c85fc 121 // send all bytes of the firmware
lilac0112_1 0:ea35c18c85fc 122 for(int i = 0; i < firmware_length; i++)
lilac0112_1 0:ea35c18c85fc 123 {
lilac0112_1 0:ea35c18c85fc 124 m_spi.write(firmware_data[i]);
lilac0112_1 0:ea35c18c85fc 125 wait_us(15);
lilac0112_1 0:ea35c18c85fc 126 }
lilac0112_1 0:ea35c18c85fc 127
lilac0112_1 0:ea35c18c85fc 128 com_end();
lilac0112_1 0:ea35c18c85fc 129 }
lilac0112_1 0:ea35c18c85fc 130
lilac0112_1 0:ea35c18c85fc 131 /**
lilac0112_1 0:ea35c18c85fc 132 * @brief starts the sensor up
lilac0112_1 0:ea35c18c85fc 133 */
lilac0112_1 0:ea35c18c85fc 134 void adns_9800::startup()
lilac0112_1 0:ea35c18c85fc 135 {
lilac0112_1 0:ea35c18c85fc 136 com_end(); // ensure that the serial port is reset
lilac0112_1 0:ea35c18c85fc 137 com_begin(); // ensure that the serial port is reset
lilac0112_1 0:ea35c18c85fc 138 com_end(); // ensure that the serial port is reset
lilac0112_1 0:ea35c18c85fc 139 write_reg(REG_Power_Up_Reset, 0x5a); // force reset
lilac0112_1 0:ea35c18c85fc 140 wait_ms(50); // wait for it to reboot
lilac0112_1 0:ea35c18c85fc 141 // read registers 0x02 to 0x06 (and discard the data)
lilac0112_1 0:ea35c18c85fc 142 read_reg(REG_Motion);
lilac0112_1 0:ea35c18c85fc 143 read_reg(REG_Delta_X_L);
lilac0112_1 0:ea35c18c85fc 144 read_reg(REG_Delta_X_H);
lilac0112_1 0:ea35c18c85fc 145 read_reg(REG_Delta_Y_L);
lilac0112_1 0:ea35c18c85fc 146 read_reg(REG_Delta_Y_H);
lilac0112_1 0:ea35c18c85fc 147 // upload the firmware
lilac0112_1 0:ea35c18c85fc 148 upload_firmware();
lilac0112_1 0:ea35c18c85fc 149 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 150 //enable laser(bit 0 = 0b), in normal mode (bits 3,2,1 = 000b)
lilac0112_1 0:ea35c18c85fc 151 // reading the actual value of the register is important because the real
lilac0112_1 0:ea35c18c85fc 152 // default value is different from what is said in the datasheet, and if you
lilac0112_1 0:ea35c18c85fc 153 // change the reserved bytes (like by writing 0x00...) it would not work.
lilac0112_1 0:ea35c18c85fc 154 uint8_t laser_ctrl0 = read_reg(REG_LASER_CTRL0);
lilac0112_1 0:ea35c18c85fc 155 write_reg(REG_LASER_CTRL0, laser_ctrl0 & 0xf0 );
lilac0112_1 0:ea35c18c85fc 156
lilac0112_1 0:ea35c18c85fc 157 wait_ms(1);
lilac0112_1 0:ea35c18c85fc 158 }