Final Project / Mbed 2 deprecated mbed-os-Final

This is a very simple guide, reviewing the steps required to get Blinky working on an Mbed OS platform.

Dependencies:   mbed Adafruit_GFX

TRSensors.cpp@96:7465ab270e7a, 2019-06-13 (annotated)

Committer:
ParkChunMyong
Date:
Thu Jun 13 03:26:42 2019 +0000
Revision:
96:7465ab270e7a
Parent:
95:250afd53b710 
first init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ParkChunMyong 95:250afd53b710 1 #include "TRSensors.h"
ParkChunMyong 95:250afd53b710 2
ParkChunMyong 95:250afd53b710 3 #define NUMSENSORS 5
ParkChunMyong 95:250afd53b710 4
ParkChunMyong 95:250afd53b710 5 TRSensors::TRSensors(PinName p1, PinName p2, PinName p3, PinName p4): spi(p2, p3, p4), cs(p1, 1){
ParkChunMyong 95:250afd53b710 6 _numSensors = NUMSENSORS;
ParkChunMyong 95:250afd53b710 7
ParkChunMyong 95:250afd53b710 8 calibratedMin = (uint16_t*)malloc(sizeof(uint16_t) * _numSensors);
ParkChunMyong 95:250afd53b710 9 calibratedMax = (uint16_t*)malloc(sizeof(uint16_t) * _numSensors);
ParkChunMyong 95:250afd53b710 10
ParkChunMyong 95:250afd53b710 11 for(int i=0;i<_numSensors;i++)
ParkChunMyong 95:250afd53b710 12 {
ParkChunMyong 95:250afd53b710 13 calibratedMin[i] = 1023;
ParkChunMyong 95:250afd53b710 14 calibratedMax[i] = 0;
ParkChunMyong 95:250afd53b710 15 }
ParkChunMyong 95:250afd53b710 16
ParkChunMyong 95:250afd53b710 17 spi.format(16,0);
ParkChunMyong 95:250afd53b710 18 spi.frequency(2000000);
ParkChunMyong 95:250afd53b710 19 }
ParkChunMyong 95:250afd53b710 20
ParkChunMyong 95:250afd53b710 21 TRSensors::~TRSensors(){
ParkChunMyong 95:250afd53b710 22 free(calibratedMin);
ParkChunMyong 95:250afd53b710 23 free(calibratedMax);
ParkChunMyong 95:250afd53b710 24 }
ParkChunMyong 95:250afd53b710 25
ParkChunMyong 95:250afd53b710 26 void TRSensors::AnalogRead(uint16_t *sensor_value){
ParkChunMyong 95:250afd53b710 27 unsigned int channel;
ParkChunMyong 95:250afd53b710 28 uint16_t tmp[6];
ParkChunMyong 95:250afd53b710 29
ParkChunMyong 95:250afd53b710 30 for(channel = 0; channel < 6; channel++)
ParkChunMyong 95:250afd53b710 31 {
ParkChunMyong 95:250afd53b710 32 cs=0;
ParkChunMyong 95:250afd53b710 33 wait_us(2);
ParkChunMyong 95:250afd53b710 34 tmp[channel] = spi.write(channel<<12);
ParkChunMyong 95:250afd53b710 35 cs=1;
ParkChunMyong 95:250afd53b710 36 wait_us(21);
ParkChunMyong 95:250afd53b710 37 }
ParkChunMyong 95:250afd53b710 38
ParkChunMyong 95:250afd53b710 39 for(int i=1; i<=5; i++)
ParkChunMyong 95:250afd53b710 40 sensor_value[i-1] = (tmp[i]>>6);
ParkChunMyong 95:250afd53b710 41 }
ParkChunMyong 95:250afd53b710 42
ParkChunMyong 95:250afd53b710 43 void TRSensors::calibrate(){
ParkChunMyong 95:250afd53b710 44 int i, j;
ParkChunMyong 95:250afd53b710 45 uint16_t sensor_values[_numSensors];
ParkChunMyong 95:250afd53b710 46 uint16_t max_sensor_values[_numSensors];
ParkChunMyong 95:250afd53b710 47 uint16_t min_sensor_values[_numSensors];
ParkChunMyong 95:250afd53b710 48
ParkChunMyong 95:250afd53b710 49 for(j=0;j<10;j++)
ParkChunMyong 95:250afd53b710 50 {
ParkChunMyong 95:250afd53b710 51 AnalogRead(sensor_values);
ParkChunMyong 95:250afd53b710 52 for(i=0;i<_numSensors;i++)
ParkChunMyong 95:250afd53b710 53 {
ParkChunMyong 95:250afd53b710 54 // set the max we found THIS time
ParkChunMyong 95:250afd53b710 55 if(j == 0 || max_sensor_values[i] < sensor_values[i])
ParkChunMyong 95:250afd53b710 56 max_sensor_values[i] = sensor_values[i];
ParkChunMyong 95:250afd53b710 57
ParkChunMyong 95:250afd53b710 58 // set the min we found THIS time
ParkChunMyong 95:250afd53b710 59 if(j == 0 || min_sensor_values[i] > sensor_values[i])
ParkChunMyong 95:250afd53b710 60 min_sensor_values[i] = sensor_values[i];
ParkChunMyong 95:250afd53b710 61 }
ParkChunMyong 95:250afd53b710 62 }
ParkChunMyong 95:250afd53b710 63
ParkChunMyong 95:250afd53b710 64 for(i=0;i<_numSensors;i++)
ParkChunMyong 95:250afd53b710 65 {
ParkChunMyong 95:250afd53b710 66 if(min_sensor_values[i] > calibratedMax[i])
ParkChunMyong 95:250afd53b710 67 calibratedMax[i] = min_sensor_values[i];
ParkChunMyong 95:250afd53b710 68 if(max_sensor_values[i] < calibratedMin[i])
ParkChunMyong 95:250afd53b710 69 calibratedMin[i] = max_sensor_values[i];
ParkChunMyong 95:250afd53b710 70 }
ParkChunMyong 95:250afd53b710 71 }
ParkChunMyong 95:250afd53b710 72
ParkChunMyong 95:250afd53b710 73 void TRSensors::readCalibrated(uint16_t *sensor_values){
ParkChunMyong 95:250afd53b710 74 int i;
ParkChunMyong 95:250afd53b710 75
ParkChunMyong 95:250afd53b710 76 // read the needed values
ParkChunMyong 95:250afd53b710 77 AnalogRead(sensor_values);
ParkChunMyong 95:250afd53b710 78
ParkChunMyong 95:250afd53b710 79 for(i=0;i<_numSensors;i++)
ParkChunMyong 95:250afd53b710 80 {
ParkChunMyong 95:250afd53b710 81 uint16_t denominator;
ParkChunMyong 95:250afd53b710 82
ParkChunMyong 95:250afd53b710 83 denominator = calibratedMax[i] - calibratedMin[i];
ParkChunMyong 95:250afd53b710 84
ParkChunMyong 95:250afd53b710 85 uint16_t x = 0;
ParkChunMyong 95:250afd53b710 86 if(denominator != 0)
ParkChunMyong 95:250afd53b710 87 x = (sensor_values[i] - calibratedMin[i])
ParkChunMyong 95:250afd53b710 88 * 1000 / denominator;
ParkChunMyong 95:250afd53b710 89 if(x < 0)
ParkChunMyong 95:250afd53b710 90 x = 0;
ParkChunMyong 95:250afd53b710 91 else if(x > 1000)
ParkChunMyong 95:250afd53b710 92 x = 1000;
ParkChunMyong 95:250afd53b710 93 sensor_values[i] = x;
ParkChunMyong 95:250afd53b710 94 }
ParkChunMyong 95:250afd53b710 95 }
ParkChunMyong 95:250afd53b710 96
ParkChunMyong 95:250afd53b710 97 uint16_t TRSensors::readLine(uint16_t *sensor_values, unsigned char white_line){
ParkChunMyong 95:250afd53b710 98 unsigned char i, on_line = 0;
ParkChunMyong 95:250afd53b710 99 unsigned long avg; // this is for the weighted total, which is long
ParkChunMyong 95:250afd53b710 100 // before division
ParkChunMyong 95:250afd53b710 101 int sum; // this is for the denominator which is <= 64000
ParkChunMyong 95:250afd53b710 102 static int last_value=0; // assume initially that the line is left.
ParkChunMyong 95:250afd53b710 103
ParkChunMyong 95:250afd53b710 104 readCalibrated(sensor_values);
ParkChunMyong 95:250afd53b710 105
ParkChunMyong 95:250afd53b710 106 avg = 0;
ParkChunMyong 95:250afd53b710 107 sum = 0;
ParkChunMyong 95:250afd53b710 108
ParkChunMyong 95:250afd53b710 109 for(i=0;i<_numSensors;i++) {
ParkChunMyong 95:250afd53b710 110 int value = (int)sensor_values[i];
ParkChunMyong 95:250afd53b710 111
ParkChunMyong 95:250afd53b710 112 if(!white_line)
ParkChunMyong 95:250afd53b710 113 value = 1000-value;
ParkChunMyong 95:250afd53b710 114 sensor_values[i] = value;
ParkChunMyong 95:250afd53b710 115 // keep track of whether we see the line at all
ParkChunMyong 95:250afd53b710 116 if(value > 300) {
ParkChunMyong 95:250afd53b710 117 on_line = 1;
ParkChunMyong 95:250afd53b710 118 }
ParkChunMyong 95:250afd53b710 119
ParkChunMyong 95:250afd53b710 120 // only average in values that are above a noise threshold
ParkChunMyong 95:250afd53b710 121 if(value > 50) {
ParkChunMyong 95:250afd53b710 122 avg += (long)(value) * (i * 1000);
ParkChunMyong 95:250afd53b710 123 sum += value;
ParkChunMyong 95:250afd53b710 124 }
ParkChunMyong 95:250afd53b710 125 }
ParkChunMyong 95:250afd53b710 126
ParkChunMyong 95:250afd53b710 127 if(!on_line)
ParkChunMyong 95:250afd53b710 128 {
ParkChunMyong 95:250afd53b710 129 // If it last read to the left of center, return 0.
ParkChunMyong 95:250afd53b710 130 if(last_value < (_numSensors-1)*1000/2)
ParkChunMyong 95:250afd53b710 131 return 0;
ParkChunMyong 95:250afd53b710 132
ParkChunMyong 95:250afd53b710 133 // If it last read to the right of center, return the max.
ParkChunMyong 95:250afd53b710 134 else
ParkChunMyong 95:250afd53b710 135 return (_numSensors-1)*1000;
ParkChunMyong 95:250afd53b710 136 }
ParkChunMyong 95:250afd53b710 137
ParkChunMyong 95:250afd53b710 138 last_value = avg/sum;
ParkChunMyong 95:250afd53b710 139
ParkChunMyong 95:250afd53b710 140 return last_value;
ParkChunMyong 95:250afd53b710 141 }