Soft robot team / Mbed OS Robot_team1

Robot team project

Dependencies:   QEI Motordriver ros_lib_melodic

sensor/Sensor.cpp@5:8ef79eebbc97, 2019-10-22 (annotated)

Committer:
florine_van
Date:
Tue Oct 22 15:06:44 2019 +0000
Revision:
5:8ef79eebbc97
Parent:
4:cb50c6fa340b 
Implementation of motor class; Link to sensors in main method

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