Team 9 / LineScan

Moved to Team 9.

Fork of LineScan by Nicholas Gan

LineScan.cpp@2:6a87b2348245, 2015-03-19 (annotated)

Committer:
ng3600
Date:
Thu Mar 19 19:13:34 2015 +0000
Revision:
2:6a87b2348245
Parent:
1:f10ec868cd71
Child:
3:f31986cb68fd
Implemented PI controller for servo. Linescan camera is able to acquire line and report position to controller which decides on what steering angle to input.; ; Servos refuse to actuate and steering control parameters need tuning.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ng3600 0:2d546112b0b8 1 /*
ng3600 0:2d546112b0b8 2 AnalogIn cam1(CAM1_IN);
ng3600 0:2d546112b0b8 3 DigitalOut camSi(CAM_SI);
ikrase 1:f10ec868cd71 4 DigitalOut camClk(CAM_CLK); // Definining camera pins. These are actually defined in Main...
ng3600 0:2d546112b0b8 5 */
ng3600 0:2d546112b0b8 6 #include "LineScan.h"
ng3600 0:2d546112b0b8 7
ng3600 0:2d546112b0b8 8 uint16_t read1Bit(AnalogIn cam, DigitalOut *camClk){
ng3600 0:2d546112b0b8 9 uint16_t pixel;
ng3600 0:2d546112b0b8 10
ng3600 0:2d546112b0b8 11 //read pixel n
ng3600 0:2d546112b0b8 12 pixel = cam.read_u16();
ng3600 0:2d546112b0b8 13
ng3600 0:2d546112b0b8 14 //clock pulse for next pixel n + 1
ng3600 0:2d546112b0b8 15 *camClk = 1;
ikrase 1:f10ec868cd71 16 *camClk = 0; // Apparently there is no need for any delay. It may be desireable to flatten this, or perhaps the compiler does it.
ng3600 0:2d546112b0b8 17
ng3600 0:2d546112b0b8 18 return pixel; //return result as an uint16_t (16 bit integer)
ng3600 0:2d546112b0b8 19 }
ng3600 0:2d546112b0b8 20
ng3600 0:2d546112b0b8 21 void startRead(DigitalOut *camSi, DigitalOut *camClk){
ng3600 0:2d546112b0b8 22 //pulse first clock cycle and start integral pin
ng3600 0:2d546112b0b8 23 *camSi = 1;
ng3600 0:2d546112b0b8 24 *camClk = 1;
ng3600 0:2d546112b0b8 25 *camSi = 0;
ng3600 0:2d546112b0b8 26 *camClk = 0;
ng3600 0:2d546112b0b8 27 }
ng3600 0:2d546112b0b8 28
ng3600 0:2d546112b0b8 29 int processFn(uint16_t *array, int arraySz){
ng3600 2:6a87b2348245 30 int avg[NUM_PIX];
ng3600 2:6a87b2348245 31 int diff[NUM_PIX];
ng3600 2:6a87b2348245 32 int highest = 0;
ng3600 2:6a87b2348245 33 int lowest = 0;
ng3600 2:6a87b2348245 34 int h_idx = NUM_PIX/2;
ng3600 2:6a87b2348245 35 int l_idx = NUM_PIX/2;
ng3600 0:2d546112b0b8 36
ng3600 2:6a87b2348245 37 //Just finds line center, does not track crossings
ng3600 2:6a87b2348245 38 if(array){
ng3600 2:6a87b2348245 39 avg[0] = array[0];
ng3600 2:6a87b2348245 40 diff[0] = 0;
ng3600 2:6a87b2348245 41
ng3600 2:6a87b2348245 42 for(int i = 1; i < NUM_PIX; i++){
ng3600 2:6a87b2348245 43 avg[i] = array[i - 1]/2 + array[i]/2;
ng3600 2:6a87b2348245 44 diff[i] = avg[i - 1] - avg[i];
ng3600 2:6a87b2348245 45
ng3600 2:6a87b2348245 46 if(diff[i] > highest){
ng3600 2:6a87b2348245 47 highest = diff[i];
ng3600 2:6a87b2348245 48 h_idx = i;
ng3600 2:6a87b2348245 49 }
ng3600 2:6a87b2348245 50 else if(diff[i] < lowest){
ng3600 2:6a87b2348245 51 lowest = diff[i];
ng3600 2:6a87b2348245 52 l_idx = i;
ng3600 2:6a87b2348245 53 }
ng3600 2:6a87b2348245 54 }
ng3600 0:2d546112b0b8 55 }
ng3600 0:2d546112b0b8 56
ng3600 2:6a87b2348245 57 return (h_idx + l_idx)/2;
ng3600 0:2d546112b0b8 58 }
ng3600 0:2d546112b0b8 59
ng3600 0:2d546112b0b8 60 //call after integration time is done, returns index of array line is expected to be at
ng3600 0:2d546112b0b8 61 int getLinePos(AnalogIn cam, DigitalOut *camSi, DigitalOut *camClk){
ikrase 1:f10ec868cd71 62 uint16_t lineAry[NUM_PIX]; // It might be nice to make this static and add double buffering and export of pointers like I had.
ng3600 0:2d546112b0b8 63 int position;
ng3600 0:2d546112b0b8 64
ng3600 0:2d546112b0b8 65 //read
ng3600 0:2d546112b0b8 66 startRead(camSi, camClk);
ng3600 0:2d546112b0b8 67 for(int i = 0; i < NUM_PIX; i++){
ng3600 0:2d546112b0b8 68 lineAry[i] = read1Bit(cam, camClk);
ng3600 0:2d546112b0b8 69 }
ng3600 0:2d546112b0b8 70
ng3600 0:2d546112b0b8 71 //process line scan data
ng3600 0:2d546112b0b8 72 position = processFn(lineAry, NUM_PIX);
ng3600 0:2d546112b0b8 73
ng3600 0:2d546112b0b8 74 return position;
ng3600 0:2d546112b0b8 75 }
ng3600 0:2d546112b0b8 76
ng3600 0:2d546112b0b8 77 /*
ng3600 0:2d546112b0b8 78 sample call (handler thread):
ng3600 0:2d546112b0b8 79
ng3600 0:2d546112b0b8 80 while(1){
ng3600 0:2d546112b0b8 81 linePos = getLinePos(cameraIn1, si, clk); //volatile linePos, replace with steering angle and read 2 cameras?
ng3600 0:2d546112b0b8 82 Thread::wait(14); //sleep for 14 us
ng3600 0:2d546112b0b8 83 }
ng3600 2:6a87b2348245 84 */