Diff: newAlgorithm.cpp

Revision:

40:cdb079fc9d0a

Child:

41:79ccb6f70ab6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/newAlgorithm.cpp	Mon Mar 09 19:27:22 2015 +0000
@@ -0,0 +1,190 @@
+#include "TFC.h"
+#include <iostream>
+#include <stdio.h>
+
+const float proportional = 0;
+const float last_proportional = 0;
+const float integral = 0;
+const float derivative = 0;
+const float output = 0;
+
+const float kp = 0.05;
+const float ki = 0.02;
+const float kd = 0.02;
+
+const int set_point = 63;
+const int position = 0;
+const int error = 0;
+
+const float AGGRESSIVE = .60;
+const float MODERATE =.45;
+const float CONSERVATIVE =.30;
+const float STOP =0;
+
+const int black_values_list[LINE_SCAN_LENGTH];
+const int black_value_count = 0;
+const int black_center_value = 0;
+const int sum_black = 0;
+const int violence_level = 0;
+
+const float current_left_motor_speed = 0;
+const float current_right_motor_speed = 0;
+
+const float current_right_max = 0;
+const float current_left_max = 0;
+
+
+int main()
+{
+    TFC_Init();
+    if(rear_motor_enable_flag) 
+    {
+        TFC_HBRIDGE_ENABLE;
+
+        //current_left_motor_speed    = (TFC_ReadPot(0));
+        //current_right_motor_speed   = (TFC_ReadPot(1));
+
+        // checking behavior level
+        violence_level =  int(TFC_GetDIP_Switch());
+
+        if (violence_level==3) {
+            current_left_motor_speed  = -(AGGRESSIVE);
+            current_right_motor_speed = AGGRESSIVE;
+            current_right_max = AGGRESSIVE;
+            current_left_max = -AGGRESSIVE;
+            
+        }
+        else if (violence_level==2) {
+            current_left_motor_speed  = -(MODERATE);
+            current_right_motor_speed = (MODERATE);
+            current_right_max = MODERATE;
+            current_left_max = -MODERATE;
+            Kp = Kp * 0.75;
+            Ki = Ki * 0.75;
+            Kd = Kd * 0.75;
+        }
+        else if (violence_level==1) {
+            current_left_motor_speed  = -(CONSERVATIVE);
+            current_right_motor_speed = CONSERVATIVE;
+            current_right_max = CONSERVATIVE;
+            current_left_max = CONSERVATIVE;
+            Kp = Kp * 0.5;
+            Ki = Ki * 0.5;
+            Kd = Kd * 0.5;
+        }
+        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
+    
+}
+        
+    void loop()
+    {
+        if (linescan_enable) 
+        {
+            if (TFC_LineScanImageReady !=0) 
+            {
+                if (linescan_ping_pong) 
+                {
+                    position = getSensorValues();
+                    error = pid_calc(position);
+                    calcMotors(error);
+                }
+            }
+        }
+        else
+        {
+            TFC_SetMotorPWM(STOP, STOP);
+        }
+    }
+    
+    int getSensorValues()
+    {        
+        for (uint16_t i=0; i<128; i++) 
+        {
+            if ((*(TFC_LineScanImage0+i) < 250)) 
+            {
+                black_values_list[black_value_count] = i;
+                black_value_count++;
+            }
+        }
+
+        for(int i=0; i<black_value_count; i++) 
+        {
+            sum_black += black_values_list[i];
+        }
+                    
+            black_center_value = int(sum_black / black_value_count);
+            return black_center_value;
+        }
+    }
+
+    int pid_calc(int pos)
+    {
+        position = pos;
+        proportional = position - set_point;
+        integral = integral + proportional;
+        derivative= proportional - last_proportional;
+        last_proportional = proportional;
+        error = proportional*Kp + integral*Ki + derivative*Kd;
+    }
+    
+    void calcMotors(float error)
+    {
+        TFC_SetServo(0, error);
+        
+        if(error < 0)
+        {
+            current_left_motor_speed = current_left_motor_speed + error; // reverse this
+            
+            // make sure not going too fast
+            if(current_right_motor_speed > current_right_max)
+            {
+                current_right_motor_speed = current_right_max;
+            }
+            else
+            {
+                current_right_motor_speed = current_right_motor_speed + error;
+            }
+        }
+        else
+        { 
+            current_right_motor_speed = current_right_motor_speed - error;
+            
+            // make sure not going too fast
+            if(current_left_motor_speed < current_left_max)
+            {
+                current_left_motor_speed = current_left_max;  
+            }
+            else
+            {
+                current_left_motor_speed = current_left_motor_speed - error;
+            }
+        }
+        
+        TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
+    }
\ No newline at end of file