Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

sensor/Sensor.cpp@2:c537f1ebad7b, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 10:05:12 2019 +0000
Revision:
2:c537f1ebad7b
Parent:
1:d5adc483bce0
Child:
3:a3144a45f44c
Working code for 4 sensors

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