Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

sensor/Sensor.cpp@4:cb50c6fa340b, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 11:52:32 2019 +0000
Revision:
4:cb50c6fa340b
Parent:
3:a3144a45f44c
Child:
5:8ef79eebbc97
Use float instead of integer for sensor readings

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