Zhengguo Sheng / Mbed 2 deprecated 2017_NXP_car

mbed_intelligent_car

Dependencies:   FRDM-TFC mbed

Fork of 2017_NXP_car_Max_Ellery_V4_3 by Zhengguo Sheng

main.cpp@0:0de05459de46, 2017-04-25 (annotated)

Committer:
MaxEllery
Date:
Tue Apr 25 13:55:28 2017 +0000
Revision:
0:0de05459de46
Script used for the Exhibition of a project performed at the University of Sussex.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaxEllery 0:0de05459de46 1 #include "mbed.h"
MaxEllery 0:0de05459de46 2 #include "TFC.h"
MaxEllery 0:0de05459de46 3
MaxEllery 0:0de05459de46 4 int CameraAlgorithm_1(int *CameraArray);
MaxEllery 0:0de05459de46 5 int CameraAlgorithm_2(int *CameraArray);
MaxEllery 0:0de05459de46 6 int LineSensor[8] = {0};
MaxEllery 0:0de05459de46 7 int servoController(int *LineSensor);
MaxEllery 0:0de05459de46 8
MaxEllery 0:0de05459de46 9 Serial pc(USBTX, USBRX);
MaxEllery 0:0de05459de46 10 int main() {
MaxEllery 0:0de05459de46 11 TFC_Init();
MaxEllery 0:0de05459de46 12 int CameraArray[128]= {0};
MaxEllery 0:0de05459de46 13 uint32_t i = 0;
MaxEllery 0:0de05459de46 14
MaxEllery 0:0de05459de46 15 for(;;){
MaxEllery 0:0de05459de46 16 int Switch_Position = TFC_GetDIP_Switch();
MaxEllery 0:0de05459de46 17 float ReadPot0 = TFC_ReadPot(0);
MaxEllery 0:0de05459de46 18 float ReadPot1 = TFC_ReadPot(1);
MaxEllery 0:0de05459de46 19 int LineThreshold = (ReadPot0 * 1000);
MaxEllery 0:0de05459de46 20
MaxEllery 0:0de05459de46 21 switch(Switch_Position)
MaxEllery 0:0de05459de46 22 {
MaxEllery 0:0de05459de46 23 default:
MaxEllery 0:0de05459de46 24 case 0: //T- BATTERY VALUE/STANDBY DEFAULT
MaxEllery 0:0de05459de46 25 TFC_HBRIDGE_DISABLE;
MaxEllery 0:0de05459de46 26 if(TFC_PUSH_BUTTON_1_PRESSED){
MaxEllery 0:0de05459de46 27 float BatteryLevel = TFC_ReadBatteryVoltage();
MaxEllery 0:0de05459de46 28 pc.printf("Battery = %1.2f\r\n",BatteryLevel);
MaxEllery 0:0de05459de46 29 if(BatteryLevel > 1.4){
MaxEllery 0:0de05459de46 30 TFC_SetBatteryLED_Level(4);
MaxEllery 0:0de05459de46 31 }
MaxEllery 0:0de05459de46 32 else if(BatteryLevel > 1.1){
MaxEllery 0:0de05459de46 33 TFC_SetBatteryLED_Level(3);
MaxEllery 0:0de05459de46 34 }
MaxEllery 0:0de05459de46 35 else if(BatteryLevel > 0.8){
MaxEllery 0:0de05459de46 36 TFC_SetBatteryLED_Level(2);
MaxEllery 0:0de05459de46 37 }
MaxEllery 0:0de05459de46 38 else{
MaxEllery 0:0de05459de46 39 TFC_SetBatteryLED_Level(1);
MaxEllery 0:0de05459de46 40 }
MaxEllery 0:0de05459de46 41 wait(1);
MaxEllery 0:0de05459de46 42 TFC_SetBatteryLED_Level(0);
MaxEllery 0:0de05459de46 43 }
MaxEllery 0:0de05459de46 44 pc.printf("Standby Mode\r\n");
MaxEllery 0:0de05459de46 45 break;
MaxEllery 0:0de05459de46 46
MaxEllery 0:0de05459de46 47 case 1: //DC MOTORS ON POTS
MaxEllery 0:0de05459de46 48 TFC_HBRIDGE_ENABLE;
MaxEllery 0:0de05459de46 49 TFC_SetMotorPWM(ReadPot1,ReadPot1);
MaxEllery 0:0de05459de46 50 break;
MaxEllery 0:0de05459de46 51
MaxEllery 0:0de05459de46 52 case 2: //T- CAMERA 1/0
MaxEllery 0:0de05459de46 53 if (TFC_LineScanImageReady > 0){
MaxEllery 0:0de05459de46 54 TFC_LineScanImageReady = 0;
MaxEllery 0:0de05459de46 55 pc.printf("Threshold = %d.\r\n", LineThreshold);
MaxEllery 0:0de05459de46 56 pc.printf("Camera Array: ");
MaxEllery 0:0de05459de46 57 for(i = 0; i < 128; i++)
MaxEllery 0:0de05459de46 58 {
MaxEllery 0:0de05459de46 59 if ((int)TFC_LineScanImage0[i] < LineThreshold) {
MaxEllery 0:0de05459de46 60 CameraArray[i] = 1;
MaxEllery 0:0de05459de46 61 }
MaxEllery 0:0de05459de46 62 else {
MaxEllery 0:0de05459de46 63 CameraArray[i] = 0;
MaxEllery 0:0de05459de46 64 }
MaxEllery 0:0de05459de46 65 pc.printf("%d, ",CameraArray[i]);
MaxEllery 0:0de05459de46 66 }
MaxEllery 0:0de05459de46 67 pc.printf("\r\n");
MaxEllery 0:0de05459de46 68 wait(1);
MaxEllery 0:0de05459de46 69 }
MaxEllery 0:0de05459de46 70 break;
MaxEllery 0:0de05459de46 71
MaxEllery 0:0de05459de46 72 case 3: //T- CAMERA 16x8
MaxEllery 0:0de05459de46 73 if (TFC_LineScanImageReady > 0){
MaxEllery 0:0de05459de46 74 TFC_LineScanImageReady = 0;
MaxEllery 0:0de05459de46 75 pc.printf("Threshold = %d.\r\n", LineThreshold);
MaxEllery 0:0de05459de46 76 pc.printf("Camera Array: ");
MaxEllery 0:0de05459de46 77 for(i = 0; i < 128; i++)
MaxEllery 0:0de05459de46 78 {
MaxEllery 0:0de05459de46 79 if ((int)TFC_LineScanImage0[i] < LineThreshold) {
MaxEllery 0:0de05459de46 80 CameraArray[i] = 1;
MaxEllery 0:0de05459de46 81 }
MaxEllery 0:0de05459de46 82 else {
MaxEllery 0:0de05459de46 83 CameraArray[i] = 0;
MaxEllery 0:0de05459de46 84 }
MaxEllery 0:0de05459de46 85 }
MaxEllery 0:0de05459de46 86 CameraAlgorithm_1(CameraArray);
MaxEllery 0:0de05459de46 87 wait(0.5);
MaxEllery 0:0de05459de46 88 }
MaxEllery 0:0de05459de46 89 break;
MaxEllery 0:0de05459de46 90
MaxEllery 0:0de05459de46 91 case 4: //T- CAMERA 8x1 1/0
MaxEllery 0:0de05459de46 92 if (TFC_LineScanImageReady > 0){
MaxEllery 0:0de05459de46 93 TFC_LineScanImageReady = 0;
MaxEllery 0:0de05459de46 94 pc.printf("Threshold = %d.\r\n", LineThreshold);
MaxEllery 0:0de05459de46 95 pc.printf("Camera Array: ");
MaxEllery 0:0de05459de46 96 for(i = 0; i < 128; i++)
MaxEllery 0:0de05459de46 97 {
MaxEllery 0:0de05459de46 98 if ((int)TFC_LineScanImage0[i] < LineThreshold) {
MaxEllery 0:0de05459de46 99 CameraArray[i] = 1;
MaxEllery 0:0de05459de46 100 }
MaxEllery 0:0de05459de46 101 else {
MaxEllery 0:0de05459de46 102 CameraArray[i] = 0;
MaxEllery 0:0de05459de46 103 }
MaxEllery 0:0de05459de46 104 }
MaxEllery 0:0de05459de46 105 CameraAlgorithm_2(CameraArray);
MaxEllery 0:0de05459de46 106 wait(0.5);
MaxEllery 0:0de05459de46 107 }
MaxEllery 0:0de05459de46 108 break;
MaxEllery 0:0de05459de46 109
MaxEllery 0:0de05459de46 110 case 5: //SERVO ON POTS
MaxEllery 0:0de05459de46 111 TFC_SetServo(0,ReadPot1);
MaxEllery 0:0de05459de46 112 break;
MaxEllery 0:0de05459de46 113
MaxEllery 0:0de05459de46 114 case 6: //SERVO MOVEMENT 1 LINE CAMERA
MaxEllery 0:0de05459de46 115 if (TFC_LineScanImageReady > 0){
MaxEllery 0:0de05459de46 116 TFC_LineScanImageReady = 0;
MaxEllery 0:0de05459de46 117 pc.printf("Threshold = %d.\r\n", LineThreshold);
MaxEllery 0:0de05459de46 118 pc.printf("Camera Array: ");
MaxEllery 0:0de05459de46 119 for(i = 0; i < 128; i++)
MaxEllery 0:0de05459de46 120 {
MaxEllery 0:0de05459de46 121 if ((int)TFC_LineScanImage0[i] < LineThreshold) {
MaxEllery 0:0de05459de46 122 CameraArray[i] = 1;
MaxEllery 0:0de05459de46 123 }
MaxEllery 0:0de05459de46 124 else {
MaxEllery 0:0de05459de46 125 CameraArray[i] = 0;
MaxEllery 0:0de05459de46 126 }
MaxEllery 0:0de05459de46 127 }
MaxEllery 0:0de05459de46 128 CameraAlgorithm_2(CameraArray);
MaxEllery 0:0de05459de46 129 servoController(LineSensor);
MaxEllery 0:0de05459de46 130 }
MaxEllery 0:0de05459de46 131 break;
MaxEllery 0:0de05459de46 132 }
MaxEllery 0:0de05459de46 133 }
MaxEllery 0:0de05459de46 134 }
MaxEllery 0:0de05459de46 135
MaxEllery 0:0de05459de46 136 int CameraAlgorithm_1(int *CameraArray) {
MaxEllery 0:0de05459de46 137 int Camera_Matrix[8][16]; //8 rows, 16 column array. Camera broken into segments.
MaxEllery 0:0de05459de46 138 uint32_t i, j, k=0;
MaxEllery 0:0de05459de46 139 for(i = 0; i < 8; i++) //Loop for 8 rows
MaxEllery 0:0de05459de46 140 {
MaxEllery 0:0de05459de46 141 for(j=0 ;j<16; j++) //Loop for 16 columns
MaxEllery 0:0de05459de46 142 {
MaxEllery 0:0de05459de46 143 Camera_Matrix[i][j] = CameraArray[k]; //Stores CameraArray into specific row/column of Camera_Matrix
MaxEllery 0:0de05459de46 144 k++; //increments k(CameraArray), only reset upon leaving the initial for loop
MaxEllery 0:0de05459de46 145 }
MaxEllery 0:0de05459de46 146 }
MaxEllery 0:0de05459de46 147 //Sum the binary digits, base 2, in each segment
MaxEllery 0:0de05459de46 148 for(i = 0; i < 8; i++) //select row
MaxEllery 0:0de05459de46 149 {
MaxEllery 0:0de05459de46 150 int sum = 0; //sum reset upon changing row
MaxEllery 0:0de05459de46 151 for(j=0 ;j<16; j++) //iterate through 16 columns
MaxEllery 0:0de05459de46 152 {
MaxEllery 0:0de05459de46 153 sum = sum + pow(2.0,15.0-j)*Camera_Matrix[i][j]; // sums the base 2 power, so that 0001 doesnt = 0100, allows line positioning per segment
MaxEllery 0:0de05459de46 154 }
MaxEllery 0:0de05459de46 155 LineSensor[i] = sum; //stores the binary sum in locations 0-7 in LineSensor
MaxEllery 0:0de05459de46 156 pc.printf("%d, ",LineSensor[i]);
MaxEllery 0:0de05459de46 157 }
MaxEllery 0:0de05459de46 158 pc.printf("\r\n");
MaxEllery 0:0de05459de46 159 return *LineSensor; //returns LineSensor values to main function
MaxEllery 0:0de05459de46 160 }
MaxEllery 0:0de05459de46 161
MaxEllery 0:0de05459de46 162 int CameraAlgorithm_2(int *CameraArray) {
MaxEllery 0:0de05459de46 163 int Camera_Matrix[8][16]; //8 rows, 16 column array. Camera broken into segments.
MaxEllery 0:0de05459de46 164 uint32_t i, j, k=0;
MaxEllery 0:0de05459de46 165 for(i = 0; i < 8; i++) //Loop for 8 rows
MaxEllery 0:0de05459de46 166 {
MaxEllery 0:0de05459de46 167 for(j=0 ;j<16; j++) //Loop for 16 columns
MaxEllery 0:0de05459de46 168 {
MaxEllery 0:0de05459de46 169 Camera_Matrix[i][j] = CameraArray[k]; //Stores CameraArray into specific row/column of Camera_Matrix
MaxEllery 0:0de05459de46 170 k++; //increments k(CameraArray), only reset upon leaving the initial for loop
MaxEllery 0:0de05459de46 171 }
MaxEllery 0:0de05459de46 172 }
MaxEllery 0:0de05459de46 173 //Sum the binary digits, base 2, in each segment
MaxEllery 0:0de05459de46 174 for(i = 0; i < 8; i++) //select row
MaxEllery 0:0de05459de46 175 {
MaxEllery 0:0de05459de46 176 int sum = 0; //sum reset upon changing row
MaxEllery 0:0de05459de46 177 for(j=0 ;j<16; j++) //iterate through 16 columns
MaxEllery 0:0de05459de46 178 {
MaxEllery 0:0de05459de46 179 sum = sum + pow(2.0,15.0-j)*Camera_Matrix[i][j]; // sums the base 2 power, so that 0001 doesnt = 0100, allows line positioning per segment
MaxEllery 0:0de05459de46 180 }
MaxEllery 0:0de05459de46 181 LineSensor[i] = sum; //stores the binary sum in locations 0-7 in LineSensor
MaxEllery 0:0de05459de46 182
MaxEllery 0:0de05459de46 183 if (LineSensor[i] > 500){
MaxEllery 0:0de05459de46 184 LineSensor[i] = 1;
MaxEllery 0:0de05459de46 185 }
MaxEllery 0:0de05459de46 186 else {
MaxEllery 0:0de05459de46 187 LineSensor[i] = 0;
MaxEllery 0:0de05459de46 188 }
MaxEllery 0:0de05459de46 189
MaxEllery 0:0de05459de46 190 pc.printf("%d, ",LineSensor[i]);
MaxEllery 0:0de05459de46 191 }
MaxEllery 0:0de05459de46 192 pc.printf("\r\n");
MaxEllery 0:0de05459de46 193 return *LineSensor; //returns LineSensor values to main function
MaxEllery 0:0de05459de46 194 }
MaxEllery 0:0de05459de46 195
MaxEllery 0:0de05459de46 196 int servoController(int *LineSensor) {
MaxEllery 0:0de05459de46 197 if (LineSensor[0] == 1){
MaxEllery 0:0de05459de46 198 TFC_SetServo(0, -1);
MaxEllery 0:0de05459de46 199 }
MaxEllery 0:0de05459de46 200 else if (LineSensor[7] == 1){
MaxEllery 0:0de05459de46 201 TFC_SetServo(0, 0.5);
MaxEllery 0:0de05459de46 202 }
MaxEllery 0:0de05459de46 203 else {
MaxEllery 0:0de05459de46 204 TFC_SetServo(0, 0);
MaxEllery 0:0de05459de46 205 }
MaxEllery 0:0de05459de46 206 return *LineSensor;
MaxEllery 0:0de05459de46 207 }