Diff: main.cpp

Revision:

0:113ea09a1ea9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Mar 24 15:15:15 2015 +0000
@@ -0,0 +1,204 @@
+//#include "mbed.h"
+#include "TFC.h"
+//#include <iostream>
+//#include <stdio.h>
+//#include "serialib.h"
+
+const float AGGRESSIVE = .55;
+const float MODERATE =.48;
+const float CONSERVATIVE =.35;
+const float STOP =0;
+const float PROTECTION_THRESHOLD_UPPER =.7;
+const float PROTECTION_THRESHOLD_LOWER =-.7;
+const float TURN_FORWARD_ACCEL =0.045;
+const float TURN_BACKWARD_ACCEL =0.025;
+const float SERVO_CAN_MOVE_IN_ONE_FRAME =0.1;
+const float SERVO_MAX =.5;
+
+const int BLACK_THRESHOLD =63;
+const int LINE_SCAN_LENGTH =128;
+
+
+DigitalOut myled(LED1);
+
+int main()
+{
+    //run this before anything
+    TFC_Init();
+
+    //variables
+    float current_servo_position = 0;
+    float previous_servo_position = 0;
+    float current_left_motor_speed = 0;
+    float current_right_motor_speed = 0;
+    
+    float proportional = 0;
+    float last_proportional = 0;
+    float integral = 0;
+    float derivative = 0;
+    float output = 0;
+    
+    // gains on prop, int, der
+    // subject to change, need to fine tune
+    float kp = 1.8960;
+    float ki = 0.6170;
+    float kd = 1.5590;
+    
+    bool rear_motor_enable_flag = true;
+    bool linescan_ping_pong = false;
+    bool linescan_enable = true;
+
+    int black_values_list[LINE_SCAN_LENGTH];
+    int black_value_count = 0;
+    int black_center_value = 0;
+    int sum_black = 0;
+    int violence_level = 0;
+    
+    int accelList[3];
+    int lastAccessed = 0;
+
+    int centers_List[50];
+    
+    int center_now = 63;
+    int center_past_1 = 63;
+    int center_past_2 = 63;
+    int center_past_3 = 63;
+    int center_past_4 = 63;
+    //int best_guess_center = 64;
+    
+    int position = 0;
+    int set_point = 63;
+    int previous_error = 0;
+    int error = 0;
+
+    for(int i = 0; i < 50; i++)
+        centers_List[i] = 63;
+    
+    int line_left_at;
+    int line_right_at;
+    bool line_left = false;
+    bool line_right = false;
+    int left_counter  =0;
+    int right_counter =0;
+    
+
+    // major loop
+    while(1) {
+
+        // initial motor stuff
+        if(rear_motor_enable_flag) {
+            TFC_HBRIDGE_ENABLE;
+
+            // checking behavior level
+            violence_level =  int(TFC_GetDIP_Switch());
+
+            if (violence_level==3) {
+                current_left_motor_speed  = -(AGGRESSIVE);
+                current_right_motor_speed = AGGRESSIVE;
+            }
+            else if (violence_level==2) {
+                current_left_motor_speed  = -(MODERATE);
+                current_right_motor_speed = (MODERATE);
+            }
+            else if (violence_level==1) {
+                current_left_motor_speed  = -(CONSERVATIVE);
+                current_right_motor_speed = CONSERVATIVE;
+            }
+            else if (violence_level==0) {
+                current_left_motor_speed  = STOP;
+                current_right_motor_speed = STOP;
+            }
+            else {
+                current_left_motor_speed  = STOP;
+                current_right_motor_speed = STOP;
+            }
+            
+
+            // protection block
+            if(current_left_motor_speed >= PROTECTION_THRESHOLD_UPPER)
+                current_left_motor_speed= PROTECTION_THRESHOLD_UPPER;
+            if(current_right_motor_speed >= PROTECTION_THRESHOLD_UPPER)
+                current_right_motor_speed = PROTECTION_THRESHOLD_UPPER;
+            if(current_left_motor_speed <= PROTECTION_THRESHOLD_LOWER)
+                current_left_motor_speed = PROTECTION_THRESHOLD_LOWER;
+            if(current_right_motor_speed <= PROTECTION_THRESHOLD_LOWER)
+                current_right_motor_speed = PROTECTION_THRESHOLD_LOWER;
+
+            TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
+        }// end motor enabled
+        else {
+            TFC_HBRIDGE_DISABLE;
+        }// end motor disabled
+
+        // camera stuff
+        if (linescan_enable) {
+            if (TFC_LineScanImageReady !=0) {
+
+                if (linescan_ping_pong) {
+                    //checking channel 0
+                    
+                    for (uint16_t i=100; i>0; i--) {
+                        line_left = (*(TFC_LineScanImage0+i) < 250) and (*(TFC_LineScanImage0+i-1) < 250) and (*(TFC_LineScanImage0+i-2) < 250);
+                        if (line_left) {
+                            line_left_at = i;
+                            i=-1;
+                            TFC_SetBatteryLED(1);
+                        }
+                    }
+                    
+                    for (uint16_t i=40; i<128; i++) {
+                        line_right = (*(TFC_LineScanImage1+i) < 250) and (*(TFC_LineScanImage1+i+1) < 250) and (*(TFC_LineScanImage1+i+2) < 250);
+                        if (line_right) {
+                            line_right_at = i;
+                            i=129;
+                            TFC_SetBatteryLED(4);
+                        }
+                    }
+                   
+                    
+                    // turn left
+                    if (line_right) {
+                        
+                    }
+                    
+                    // turn right
+                    if (line_left) {
+
+                    }
+
+                    // clearing values for next image processing round
+                    line_left_at=63;
+                    line_right_at=63;
+                    line_left = false;
+                    line_right = false;
+
+
+                    // end image processing
+                    linescan_ping_pong = false;
+                } // end checking channel 0
+                
+                else { //checking channel 1
+                    linescan_ping_pong = true;
+                }
+
+                TFC_LineScanImageReady = 0;  // since we used it, we reset the flag
+            }// end imageready
+        }// end linescan stuff
+    }//end while true
+}//end main
+
+
+/* shitcode
+left at bool, int
+right at bool, int
+
+left turn counter
+right turn counter
+
+if bool
+    inc counter by proportional to int (where line is)
+    turn away proportional to counter
+    multiply counter by .75 (turn less)
+    
+*/
+