GDP 4 / Mbed 2 deprecated pid-car-example

car using PID from centre line

Dependencies:   FRDM-TFC mbed CBuffer XBEE mbed_angular_speed motor2 MMA8451Q

Fork of KL25Z_Camera_Test by GDP 4

main.cpp@2:4b6f6fc84793, 2016-10-28 (annotated)

Committer:
maximusismax
Date:
Fri Oct 28 10:19:09 2016 +0000
Revision:
2:4b6f6fc84793
Parent:
1:a4883d9c75ec
Child:
3:87a5122682fa
Figuring out centre calc.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maximusismax 0:566127ca8048 1 #include "mbed.h"
maximusismax 0:566127ca8048 2 #include "TFC.h"
maximusismax 2:4b6f6fc84793 3 #define CAM_THRESHOLD 3000
maximusismax 0:566127ca8048 4
maximusismax 2:4b6f6fc84793 5 Serial pc(USBTX,USBRX);
maximusismax 0:566127ca8048 6 DigitalOut myled(LED1);
maximusismax 0:566127ca8048 7
maximusismax 0:566127ca8048 8 int main() {
maximusismax 2:4b6f6fc84793 9 //Serial config
maximusismax 2:4b6f6fc84793 10 pc.baud(57600);
maximusismax 2:4b6f6fc84793 11
maximusismax 2:4b6f6fc84793 12
maximusismax 0:566127ca8048 13 TFC_Init();
maximusismax 0:566127ca8048 14
maximusismax 0:566127ca8048 15 uint32_t i,t = 0;
maximusismax 0:566127ca8048 16 //uint32_t width = 0;
maximusismax 0:566127ca8048 17 char curr_line[129];
maximusismax 2:4b6f6fc84793 18 int leftSide, rightSide;
maximusismax 2:4b6f6fc84793 19 float centreOffset;
maximusismax 0:566127ca8048 20
maximusismax 0:566127ca8048 21 /* Ensure string is null terminated */
maximusismax 0:566127ca8048 22 curr_line[128] = 0;
maximusismax 2:4b6f6fc84793 23
maximusismax 2:4b6f6fc84793 24
maximusismax 2:4b6f6fc84793 25 char thres_line[16];
maximusismax 2:4b6f6fc84793 26 for(i = 0; i < 16; i++) {
maximusismax 2:4b6f6fc84793 27 thres_line[i] = 0;
maximusismax 2:4b6f6fc84793 28 }
maximusismax 0:566127ca8048 29
maximusismax 0:566127ca8048 30
maximusismax 2:4b6f6fc84793 31 pc.printf("Starting camera test program\n");
maximusismax 0:566127ca8048 32
maximusismax 0:566127ca8048 33 while(1) {
maximusismax 0:566127ca8048 34 //If we have an image ready
maximusismax 0:566127ca8048 35 if(TFC_LineScanImageReady>0)
maximusismax 0:566127ca8048 36 {
maximusismax 0:566127ca8048 37 //Reset image ready flag
maximusismax 0:566127ca8048 38 TFC_LineScanImageReady=0;
maximusismax 2:4b6f6fc84793 39 pc.printf("\r\n");
maximusismax 2:4b6f6fc84793 40 //pc.printf("L:");
maximusismax 0:566127ca8048 41
maximusismax 0:566127ca8048 42 //Strange thing that cycles the battery indicator
maximusismax 0:566127ca8048 43 //Leaving in as this is a good indicator that the camera test program is running
maximusismax 0:566127ca8048 44 if(t==4)
maximusismax 0:566127ca8048 45 t=0;
maximusismax 0:566127ca8048 46 else
maximusismax 1:a4883d9c75ec 47 t++;
maximusismax 0:566127ca8048 48 TFC_SetBatteryLED_Level(t);
maximusismax 2:4b6f6fc84793 49
maximusismax 2:4b6f6fc84793 50 //Byte sending
maximusismax 2:4b6f6fc84793 51 //for(i = 0; i < 128; i++) {
maximusismax 2:4b6f6fc84793 52 //curr_line[i] = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
maximusismax 2:4b6f6fc84793 53 //pc.printf("%02x", curr_line[i]);
maximusismax 2:4b6f6fc84793 54 //}
maximusismax 2:4b6f6fc84793 55 pc.printf("\n");
maximusismax 2:4b6f6fc84793 56
maximusismax 2:4b6f6fc84793 57
maximusismax 2:4b6f6fc84793 58 //NEW CENTRE DETECTION CODE
maximusismax 2:4b6f6fc84793 59 //Centre of track detection
maximusismax 2:4b6f6fc84793 60 //First, start at the centre of the 128 bit array
maximusismax 2:4b6f6fc84793 61 //Moving left, start at 63 and move 63 steps left (to get to 0)
maximusismax 2:4b6f6fc84793 62 //Moving right, start at 64 and move 63 steps right (to get to 127)
maximusismax 2:4b6f6fc84793 63
maximusismax 2:4b6f6fc84793 64 //Move leftwards, stopping where we reach the edge of the track, and record the index where this occurred
maximusismax 2:4b6f6fc84793 65 for (i = 63; i > 0; i--) {
maximusismax 2:4b6f6fc84793 66 if(TFC_LineScanImage0[i] < CAM_THRESHOLD) {
maximusismax 2:4b6f6fc84793 67 leftSide = i;
maximusismax 2:4b6f6fc84793 68 break;
maximusismax 2:4b6f6fc84793 69 }
maximusismax 0:566127ca8048 70 }
maximusismax 2:4b6f6fc84793 71
maximusismax 2:4b6f6fc84793 72 //Then, move rightwards, stopping where we reach the edge of the track, record the index where this occurred
maximusismax 2:4b6f6fc84793 73 for (i = 64; i < 127; i++) {
maximusismax 2:4b6f6fc84793 74 if(TFC_LineScanImage0[i] < CAM_THRESHOLD) {
maximusismax 2:4b6f6fc84793 75 rightSide = i;
maximusismax 2:4b6f6fc84793 76 break;
maximusismax 2:4b6f6fc84793 77 }
maximusismax 2:4b6f6fc84793 78 }
maximusismax 2:4b6f6fc84793 79
maximusismax 2:4b6f6fc84793 80 //centreOffset = (leftSide + rightSide)/2;
maximusismax 2:4b6f6fc84793 81 centreOffset = (leftSide - rightSide) / 127;
maximusismax 2:4b6f6fc84793 82
maximusismax 2:4b6f6fc84793 83 //Send offset over serial to hosted program
maximusismax 2:4b6f6fc84793 84 pc.putc('E');
maximusismax 2:4b6f6fc84793 85 pc.printf("%f", centreOffset);
maximusismax 2:4b6f6fc84793 86 //pc.printf("centre: %d\n", centreOffset);
maximusismax 2:4b6f6fc84793 87 ///END OF NEW CODE
maximusismax 2:4b6f6fc84793 88
maximusismax 1:a4883d9c75ec 89
maximusismax 1:a4883d9c75ec 90 //Old version below, we don't threshold on the FRDM board anymore
maximusismax 2:4b6f6fc84793 91 //pc.printf("0x%x\n", curr_line);
maximusismax 2:4b6f6fc84793 92
maximusismax 2:4b6f6fc84793 93
maximusismax 0:566127ca8048 94 //Loop through camera pixels
maximusismax 0:566127ca8048 95 for(i=0;i<128;i++)
maximusismax 0:566127ca8048 96 {
maximusismax 0:566127ca8048 97 //If the pixel value exceeds our threshold, print a 1
maximusismax 0:566127ca8048 98 if (TFC_LineScanImage0[i] > CAM_THRESHOLD)
maximusismax 0:566127ca8048 99 {
maximusismax 2:4b6f6fc84793 100 //curr_line[i] = '1';
maximusismax 2:4b6f6fc84793 101 thres_line[i/8] |= 1 << (i%8);
maximusismax 0:566127ca8048 102 }
maximusismax 0:566127ca8048 103 //Else, print a 0
maximusismax 0:566127ca8048 104 else
maximusismax 0:566127ca8048 105 {
maximusismax 2:4b6f6fc84793 106 //curr_line[i] = '0';
maximusismax 0:566127ca8048 107 }
maximusismax 0:566127ca8048 108 }
maximusismax 2:4b6f6fc84793 109 pc.putc('I');
maximusismax 2:4b6f6fc84793 110 for(i = 0; i < 16; i++) {
maximusismax 2:4b6f6fc84793 111 pc.putc(thres_line[i]);
maximusismax 2:4b6f6fc84793 112 }
maximusismax 0:566127ca8048 113
maximusismax 2:4b6f6fc84793 114 //pc.printf("Width: %d\n", width);
maximusismax 2:4b6f6fc84793 115 //width = 0;
maximusismax 2:4b6f6fc84793 116
maximusismax 0:566127ca8048 117
maximusismax 0:566127ca8048 118 }
maximusismax 0:566127ca8048 119 }
maximusismax 0:566127ca8048 120 }