Diff: main.cpp

Revision:

0:0de05459de46

--- /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