Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

sensor/Sensor.cpp@3:a3144a45f44c, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 10:09:05 2019 +0000
Revision:
3:a3144a45f44c
Parent:
2:c537f1ebad7b
Child:
4:cb50c6fa340b
Clean code and remove unused lines

Who changed what in which revision?

UserRevisionLine numberNew contents of line
florine_van 0:57855aafa907 1 #include"mbed.h"
florine_van 0:57855aafa907 2 #include <ros.h>
florine_van 0:57855aafa907 3
florine_van 0:57855aafa907 4 #include "Sensor.h"
florine_van 0:57855aafa907 5
florine_van 1:d5adc483bce0 6 Sensor::Sensor(PinName sda, PinName scl, PinName shdn)
florine_van 0:57855aafa907 7 : i2c(sda, scl), SHDN(shdn)
florine_van 0:57855aafa907 8 {
florine_van 2:c537f1ebad7b 9 addr = DEFAULT_DEVICE_ADDRESS << 1;
florine_van 3:a3144a45f44c 10
florine_van 3:a3144a45f44c 11 // At the beginning, turn off the device
florine_van 3:a3144a45f44c 12 turnOff();
florine_van 3:a3144a45f44c 13 wait_ms(0.5);
florine_van 0:57855aafa907 14 }
florine_van 0:57855aafa907 15
florine_van 0:57855aafa907 16 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 17 // TODO
florine_van 0:57855aafa907 18 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 19 int Sensor::init()
florine_van 0:57855aafa907 20 {
florine_van 3:a3144a45f44c 21 // Turn on the device
florine_van 3:a3144a45f44c 22 turnOn();
florine_van 3:a3144a45f44c 23
florine_van 0:57855aafa907 24 char reset;
florine_van 0:57855aafa907 25
florine_van 0:57855aafa907 26 // check to see has it be Initialised already
florine_van 0:57855aafa907 27 reset = readByte(0x016);
florine_van 0:57855aafa907 28 if (reset==1) {
florine_van 0:57855aafa907 29 // Mandatory settings : private registers
florine_van 0:57855aafa907 30 setRegister(0x0207, 0x01);
florine_van 0:57855aafa907 31 setRegister(0x0208, 0x01);
florine_van 0:57855aafa907 32 setRegister(0x0096, 0x00);
florine_van 0:57855aafa907 33 setRegister(0x0097, 0xfd);
florine_van 0:57855aafa907 34 setRegister(0x00e3, 0x01);
florine_van 0:57855aafa907 35 setRegister(0x00e4, 0x03);
florine_van 0:57855aafa907 36 setRegister(0x00e5, 0x02);
florine_van 0:57855aafa907 37 setRegister(0x00e6, 0x01);
florine_van 0:57855aafa907 38 setRegister(0x00e7, 0x03);
florine_van 0:57855aafa907 39 setRegister(0x00f5, 0x02);
florine_van 0:57855aafa907 40 setRegister(0x00d9, 0x05);
florine_van 0:57855aafa907 41 setRegister(0x00db, 0xce);
florine_van 0:57855aafa907 42 setRegister(0x00dc, 0x03);
florine_van 0:57855aafa907 43 setRegister(0x00dd, 0xf8);
florine_van 0:57855aafa907 44 setRegister(0x009f, 0x00);
florine_van 0:57855aafa907 45 setRegister(0x00a3, 0x3c);
florine_van 0:57855aafa907 46 setRegister(0x00b7, 0x00);
florine_van 0:57855aafa907 47 setRegister(0x00bb, 0x3c);
florine_van 0:57855aafa907 48 setRegister(0x00b2, 0x09);
florine_van 0:57855aafa907 49 setRegister(0x00ca, 0x09);
florine_van 0:57855aafa907 50 setRegister(0x0198, 0x01);
florine_van 0:57855aafa907 51 setRegister(0x01b0, 0x17);
florine_van 0:57855aafa907 52 setRegister(0x01ad, 0x00);
florine_van 0:57855aafa907 53 setRegister(0x00ff, 0x05);
florine_van 0:57855aafa907 54 setRegister(0x0100, 0x05);
florine_van 0:57855aafa907 55 setRegister(0x0199, 0x05);
florine_van 0:57855aafa907 56 setRegister(0x01a6, 0x1b);
florine_van 0:57855aafa907 57 setRegister(0x01ac, 0x3e);
florine_van 0:57855aafa907 58 setRegister(0x01a7, 0x1f);
florine_van 0:57855aafa907 59 setRegister(0x0030, 0x00);
florine_van 0:57855aafa907 60
florine_van 0:57855aafa907 61 setRegister(SYSTEM_FRESH_OUT_OF_RESET, 0x00); //change fresh out of set status to 0
florine_van 0:57855aafa907 62 } else {
florine_van 0:57855aafa907 63 return -1;
florine_van 0:57855aafa907 64 }
florine_van 0:57855aafa907 65 return 0;
florine_van 0:57855aafa907 66 }
florine_van 0:57855aafa907 67
florine_van 1:d5adc483bce0 68 void Sensor::changeAddress(char address)
florine_van 0:57855aafa907 69 {
florine_van 1:d5adc483bce0 70 setRegister(I2C_SLAVE_DEVICE_ADDRESS, address);
florine_van 2:c537f1ebad7b 71 addr = address << 1;
florine_van 0:57855aafa907 72 }
florine_van 0:57855aafa907 73
florine_van 0:57855aafa907 74 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 75 // New measurement
florine_van 0:57855aafa907 76 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 77 int Sensor::read()
florine_van 0:57855aafa907 78 {
florine_van 0:57855aafa907 79 int range;
florine_van 0:57855aafa907 80
florine_van 0:57855aafa907 81 // Start range measurement
florine_van 0:57855aafa907 82 startRange();
florine_van 0:57855aafa907 83
florine_van 0:57855aafa907 84 // Poll the VL6180 till new sample ready
florine_van 0:57855aafa907 85 pollRange();
florine_van 0:57855aafa907 86
florine_van 0:57855aafa907 87 // Read range result
florine_van 0:57855aafa907 88 range = readRange();
florine_van 0:57855aafa907 89
florine_van 0:57855aafa907 90 // Clear the interrupt on VL6180
florine_van 0:57855aafa907 91 clearInterrupts();
florine_van 0:57855aafa907 92
florine_van 0:57855aafa907 93 // Display result
florine_van 0:57855aafa907 94 return range;
florine_van 0:57855aafa907 95 }
florine_van 0:57855aafa907 96
florine_van 1:d5adc483bce0 97 void Sensor::startRange()
florine_van 1:d5adc483bce0 98 {
florine_van 2:c537f1ebad7b 99 writeByte(0x018,0x01);
florine_van 1:d5adc483bce0 100 }
florine_van 0:57855aafa907 101
florine_van 0:57855aafa907 102 void Sensor::pollRange()
florine_van 0:57855aafa907 103 {
florine_van 0:57855aafa907 104 char status;
florine_van 0:57855aafa907 105 char range_status;
florine_van 0:57855aafa907 106
florine_van 0:57855aafa907 107 // check the status
florine_van 0:57855aafa907 108 status = readByte(RESULT_INTERRUPT_STATUS_GPIO);
florine_van 0:57855aafa907 109 range_status = status & 0x07;
florine_van 0:57855aafa907 110
florine_van 0:57855aafa907 111 // wait for new measurement ready status
florine_van 0:57855aafa907 112 while (range_status != 0x00) {
florine_van 0:57855aafa907 113 status = readByte(RESULT_INTERRUPT_STATUS_GPIO);
florine_van 0:57855aafa907 114 range_status = status & 0x07;
florine_van 0:57855aafa907 115 }
florine_van 0:57855aafa907 116 }
florine_van 0:57855aafa907 117
florine_van 0:57855aafa907 118 int Sensor::readRange()
florine_van 0:57855aafa907 119 {
florine_van 0:57855aafa907 120 int range;
florine_van 0:57855aafa907 121 range = readByte(RESULT_RANGE_VAL);
florine_van 0:57855aafa907 122 return range;
florine_van 0:57855aafa907 123 }
florine_van 0:57855aafa907 124
florine_van 0:57855aafa907 125 void Sensor::clearInterrupts()
florine_van 0:57855aafa907 126 {
florine_van 0:57855aafa907 127 writeByte(0x015,0x07);
florine_van 0:57855aafa907 128 }
florine_van 0:57855aafa907 129
florine_van 1:d5adc483bce0 130 void Sensor::turnOff()
florine_van 1:d5adc483bce0 131 {
florine_van 1:d5adc483bce0 132 SHDN = 0;
florine_van 1:d5adc483bce0 133 }
florine_van 1:d5adc483bce0 134
florine_van 1:d5adc483bce0 135 void Sensor::turnOn()
florine_van 1:d5adc483bce0 136 {
florine_van 1:d5adc483bce0 137 SHDN = 1;
florine_van 1:d5adc483bce0 138 }
florine_van 1:d5adc483bce0 139
florine_van 0:57855aafa907 140 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 141 // Update a VL6180X register
florine_van 0:57855aafa907 142 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 143 void Sensor::setRegister(wchar_t reg, char data)
florine_van 0:57855aafa907 144 {
florine_van 0:57855aafa907 145 writeByte(reg, data);
florine_van 0:57855aafa907 146 }
florine_van 0:57855aafa907 147
florine_van 0:57855aafa907 148 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 149 // Split 16-bit register address into two bytes and write
florine_van 0:57855aafa907 150 // the address + data via I²C
florine_van 0:57855aafa907 151 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 152 void Sensor::writeByte(wchar_t reg, char data)
florine_van 0:57855aafa907 153 {
florine_van 0:57855aafa907 154 char data_write[3];
florine_van 0:57855aafa907 155 data_write[0] = (reg >> 8) & 0xFF;;
florine_van 0:57855aafa907 156 // MSB of register address
florine_van 0:57855aafa907 157 data_write[1] = reg & 0xFF;
florine_van 0:57855aafa907 158 // LSB of register address
florine_van 0:57855aafa907 159 data_write[2] = data & 0xFF;
florine_van 0:57855aafa907 160 i2c.write(addr, data_write, 3);
florine_van 0:57855aafa907 161 }
florine_van 0:57855aafa907 162
florine_van 0:57855aafa907 163 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 164 // Split 16-bit register address into two bytes and write
florine_van 0:57855aafa907 165 // required register address to VL6180 and read the data back
florine_van 0:57855aafa907 166 ///////////////////////////////////////////////////////////////////
florine_van 0:57855aafa907 167 char Sensor::readByte(wchar_t reg)
florine_van 0:57855aafa907 168 {
florine_van 0:57855aafa907 169 char data_write[2];
florine_van 0:57855aafa907 170 char data_read[1];
florine_van 0:57855aafa907 171
florine_van 0:57855aafa907 172 data_write[0] = (reg >> 8) & 0xFF; // MSB of register address
florine_van 0:57855aafa907 173 data_write[1] = reg & 0xFF; // LSB of register address
florine_van 0:57855aafa907 174
florine_van 0:57855aafa907 175 i2c.write(addr, data_write, 2);
florine_van 0:57855aafa907 176 i2c.read(addr, data_read, 1);
florine_van 0:57855aafa907 177 return data_read[0];
florine_van 0:57855aafa907 178 }