Zhengguo Sheng
mbed_intelligent_car
Fork of
2017_NXP_car_Max_Ellery_V4_3
by 
Zhengguo Sheng
Diff: main.cpp
- Revision:
- 0:0de05459de46
diff -r 000000000000 -r 0de05459de46 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 25 13:55:28 2017 +0000
@@ -0,0 +1,207 @@
+#include "mbed.h"
+#include "TFC.h"
+
+int CameraAlgorithm_1(int *CameraArray);
+int CameraAlgorithm_2(int *CameraArray);
+int LineSensor[8] = {0};
+int servoController(int *LineSensor);
+
+Serial pc(USBTX, USBRX);
+int main() {
+ TFC_Init();
+ int CameraArray[128]= {0};
+ uint32_t i = 0;
+
+ for(;;){
+ int Switch_Position = TFC_GetDIP_Switch();
+ float ReadPot0 = TFC_ReadPot(0);
+ float ReadPot1 = TFC_ReadPot(1);
+ int LineThreshold = (ReadPot0 * 1000);
+
+ switch(Switch_Position)
+ {
+ default:
+ case 0: //T- BATTERY VALUE/STANDBY DEFAULT
+ TFC_HBRIDGE_DISABLE;
+ if(TFC_PUSH_BUTTON_1_PRESSED){
+ float BatteryLevel = TFC_ReadBatteryVoltage();
+ pc.printf("Battery = %1.2f\r\n",BatteryLevel);
+ if(BatteryLevel > 1.4){
+ TFC_SetBatteryLED_Level(4);
+ }
+ else if(BatteryLevel > 1.1){
+ TFC_SetBatteryLED_Level(3);
+ }
+ else if(BatteryLevel > 0.8){
+ TFC_SetBatteryLED_Level(2);
+ }
+ else{
+ TFC_SetBatteryLED_Level(1);
+ }
+ wait(1);
+ TFC_SetBatteryLED_Level(0);
+ }
+ pc.printf("Standby Mode\r\n");
+ break;
+
+ case 1: //DC MOTORS ON POTS
+ TFC_HBRIDGE_ENABLE;
+ TFC_SetMotorPWM(ReadPot1,ReadPot1);
+ break;
+
+ case 2: //T- CAMERA 1/0
+ if (TFC_LineScanImageReady > 0){
+ TFC_LineScanImageReady = 0;
+ pc.printf("Threshold = %d.\r\n", LineThreshold);
+ pc.printf("Camera Array: ");
+ for(i = 0; i < 128; i++)
+ {
+ if ((int)TFC_LineScanImage0[i] < LineThreshold) {
+ CameraArray[i] = 1;
+ }
+ else {
+ CameraArray[i] = 0;
+ }
+ pc.printf("%d, ",CameraArray[i]);
+ }
+ pc.printf("\r\n");
+ wait(1);
+ }
+ break;
+
+ case 3: //T- CAMERA 16x8
+ if (TFC_LineScanImageReady > 0){
+ TFC_LineScanImageReady = 0;
+ pc.printf("Threshold = %d.\r\n", LineThreshold);
+ pc.printf("Camera Array: ");
+ for(i = 0; i < 128; i++)
+ {
+ if ((int)TFC_LineScanImage0[i] < LineThreshold) {
+ CameraArray[i] = 1;
+ }
+ else {
+ CameraArray[i] = 0;
+ }
+ }
+ CameraAlgorithm_1(CameraArray);
+ wait(0.5);
+ }
+ break;
+
+ case 4: //T- CAMERA 8x1 1/0
+ if (TFC_LineScanImageReady > 0){
+ TFC_LineScanImageReady = 0;
+ pc.printf("Threshold = %d.\r\n", LineThreshold);
+ pc.printf("Camera Array: ");
+ for(i = 0; i < 128; i++)
+ {
+ if ((int)TFC_LineScanImage0[i] < LineThreshold) {
+ CameraArray[i] = 1;
+ }
+ else {
+ CameraArray[i] = 0;
+ }
+ }
+ CameraAlgorithm_2(CameraArray);
+ wait(0.5);
+ }
+ break;
+
+ case 5: //SERVO ON POTS
+ TFC_SetServo(0,ReadPot1);
+ break;
+
+ case 6: //SERVO MOVEMENT 1 LINE CAMERA
+ if (TFC_LineScanImageReady > 0){
+ TFC_LineScanImageReady = 0;
+ pc.printf("Threshold = %d.\r\n", LineThreshold);
+ pc.printf("Camera Array: ");
+ for(i = 0; i < 128; i++)
+ {
+ if ((int)TFC_LineScanImage0[i] < LineThreshold) {
+ CameraArray[i] = 1;
+ }
+ else {
+ CameraArray[i] = 0;
+ }
+ }
+ CameraAlgorithm_2(CameraArray);
+ servoController(LineSensor);
+ }
+ break;
+ }
+ }
+}
+
+int CameraAlgorithm_1(int *CameraArray) {
+ int Camera_Matrix[8][16]; //8 rows, 16 column array. Camera broken into segments.
+ uint32_t i, j, k=0;
+ for(i = 0; i < 8; i++) //Loop for 8 rows
+ {
+ for(j=0 ;j<16; j++) //Loop for 16 columns
+ {
+ Camera_Matrix[i][j] = CameraArray[k]; //Stores CameraArray into specific row/column of Camera_Matrix
+ k++; //increments k(CameraArray), only reset upon leaving the initial for loop
+ }
+ }
+ //Sum the binary digits, base 2, in each segment
+ for(i = 0; i < 8; i++) //select row
+ {
+ int sum = 0; //sum reset upon changing row
+ for(j=0 ;j<16; j++) //iterate through 16 columns
+ {
+ 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
+ }
+ LineSensor[i] = sum; //stores the binary sum in locations 0-7 in LineSensor
+ pc.printf("%d, ",LineSensor[i]);
+ }
+ pc.printf("\r\n");
+ return *LineSensor; //returns LineSensor values to main function
+}
+
+int CameraAlgorithm_2(int *CameraArray) {
+ int Camera_Matrix[8][16]; //8 rows, 16 column array. Camera broken into segments.
+ uint32_t i, j, k=0;
+ for(i = 0; i < 8; i++) //Loop for 8 rows
+ {
+ for(j=0 ;j<16; j++) //Loop for 16 columns
+ {
+ Camera_Matrix[i][j] = CameraArray[k]; //Stores CameraArray into specific row/column of Camera_Matrix
+ k++; //increments k(CameraArray), only reset upon leaving the initial for loop
+ }
+ }
+ //Sum the binary digits, base 2, in each segment
+ for(i = 0; i < 8; i++) //select row
+ {
+ int sum = 0; //sum reset upon changing row
+ for(j=0 ;j<16; j++) //iterate through 16 columns
+ {
+ 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
+ }
+ LineSensor[i] = sum; //stores the binary sum in locations 0-7 in LineSensor
+
+ if (LineSensor[i] > 500){
+ LineSensor[i] = 1;
+ }
+ else {
+ LineSensor[i] = 0;
+ }
+
+ pc.printf("%d, ",LineSensor[i]);
+ }
+ pc.printf("\r\n");
+ return *LineSensor; //returns LineSensor values to main function
+}
+
+int servoController(int *LineSensor) {
+ if (LineSensor[0] == 1){
+ TFC_SetServo(0, -1);
+ }
+ else if (LineSensor[7] == 1){
+ TFC_SetServo(0, 0.5);
+ }
+ else {
+ TFC_SetServo(0, 0);
+ }
+ return *LineSensor;
+}
\ No newline at end of file