Diff: asof3_27_2015_1400h.txt

Revision:

20:548b20634fb4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/asof3_27_2015_1400h.txt	Fri Mar 27 18:27:52 2015 +0000
@@ -0,0 +1,391 @@
+//#include "mbed.h"
+#include "TFC.h"
+#include <iostream>
+#include <stdio.h>
+//#include "serialib.h"
+
+const float AGGRESSIVE = .55;
+const float MODERATE =.45;
+const float CONSERVATIVE =.33;
+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;
+    
+    float increment = 0;
+    int right_turn_count = 0;
+    int left_turn_count = 0;
+
+    for(int i = 0; i < 50; i++)
+        centers_List[i] = 63;
+    
+    float left_counter  =0;
+    float right_counter =0;
+    bool turn_left=false;
+    bool turn_right=false;
+    bool need_decel=false;
+    int num_of_straight =0;
+    int num_of_left =0;
+    int num_of_right=0;
+
+    //servo is offset zero by some bullshit number
+    float bullshit_offset = .068;
+
+    // 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;
+                increment = 0.02;
+            }
+            else if (violence_level==2) {
+                current_left_motor_speed  = -(MODERATE);
+                current_right_motor_speed = (MODERATE);
+                increment = 0.03;
+            }
+            else if (violence_level==1) {
+                current_left_motor_speed  = -(CONSERVATIVE);
+                current_right_motor_speed = CONSERVATIVE;
+                increment = 0.04;
+            }
+            else if (violence_level==0) {
+                current_left_motor_speed  = STOP;
+                current_right_motor_speed = STOP;
+                TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
+            }
+            else {
+                current_left_motor_speed  = STOP;
+                current_right_motor_speed = STOP;
+                TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
+            }
+            
+
+            // 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
+                    
+                    //checking center pixel, displays aprox value on leds
+                    uint8_t shitnum = 1;
+                    
+  
+                    // checking for center line (single line)
+                    for (uint16_t i=10; i<118; i++) {
+                        if ((*(TFC_LineScanImage0+i) < 400)) {
+                            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];
+                    }
+
+                    //update history
+                    center_past_4= center_past_3;
+                    center_past_3= center_past_2;
+                    center_past_2= center_past_1;
+                    center_past_1= center_now;
+                    
+                    
+                    //if (black_value_count>2)
+                    center_now = sum_black / black_value_count;
+                    
+                    uint8_t num = 0;
+                    
+                    if(center_now > 10 && center_now < 45) // linear func
+                        num = 1;
+                    else if(center_now >= 45 && center_now < 65) // snap
+                        num = 2;
+                    else if(center_now > 64 && center_now < 64) // nothing zone
+                        num = 15;
+                    else if(center_now >= 65 && center_now < 83) // snap
+                        num = 4;
+                    else if(center_now >= 83 && center_now < 118) // linear func
+                        num = 8;
+                    
+                    else
+                        num = 0;
+                    
+                    // get rid of garbage data sets
+                    if (black_value_count<2)
+                        num = 0;
+                    
+                    if (black_value_count>30)
+                    {
+                        while(1)
+                            TFC_SetMotorPWM(0, 0);
+                    }
+                    
+                    TFC_SetBatteryLED(num);
+                    
+                    // best guess of center based on weighted average of history
+                    black_center_value = center_now;
+  
+                    
+                    // turn left
+                    // hit wall a little bit on the right
+                    if (num==8 and right_counter <.2 ) // only if we arent turning right
+                    //if(center_now >= 110 && center_now < 118 and right_counter <.2)
+                    {
+                        //turn away a little bit for each frame that is wall
+                        if (left_counter >-.45)
+                        {
+                            //left_counter -=.05;    
+                            //left_counter -=.03; 
+                            //left_counter -= (.001428571*(128-center_now));
+                            //left_counter -= (.002*(128-center_now));
+                            if (center_now>110)
+                                left_counter -=.03; 
+                            else
+                                left_counter -=.05;   
+                        }
+                                
+                        if (left_counter <-.46)
+                        {
+                            num_of_left =0;
+                            //need_decel=true;
+                        }
+                            
+                            
+                        turn_left=true;
+                        turn_right=false;
+                    }
+                    
+                    // turn left real hard
+                    // wall is close to center on right
+                    if (num==4 and right_counter <.2) // only if we arent turning right
+                    {
+                        left_counter=-0.54;
+                        turn_left=true;
+                        turn_right=false;
+                        
+                        num_of_left =0;
+                        need_decel=true;
+                    }
+                    
+                    // turn right hard
+                    // wall is close to center on left
+                      else if (num==2 and left_counter >-.2)// only if we arent turning left
+                    {
+                        
+                        right_counter =.54;
+                        turn_left=false;
+                        turn_right=true;
+                        
+                        num_of_right =0;
+                        need_decel=true;
+                    }
+                    
+                    
+                    // turn right
+                    // hit wall a little bit on the left
+                    else if (num==1 and left_counter >-.2) // only if we arent turning left
+                    //else if ( center_now > 15 && center_now < 27 and left_counter >-.2) // only if we arent turning left
+                    {    
+                        //turn away a little bit for each frame that is wall
+                        if (right_counter <.45)
+                        {
+                            //right_counter +=.05;
+                            //right_counter +=.03;
+                            //right_counter +=(.001428571*center_now);
+                            //right_counter +=(.002*center_now);
+                            if (center_now<18)
+                                right_counter +=.03;
+                            else
+                                right_counter +=.05;
+                        }        
+              
+                        if (right_counter >.46)
+                        {
+                            num_of_right =0;
+                            //need_decel=true;
+                        }
+              
+                        turn_left=false;
+                        turn_right=true;
+                    }
+                    
+
+                   // going straight yesssss
+                   else if (turn_right == false and turn_left == false and (violence_level !=0))
+                    {
+                        TFC_SetServo(0,(0.0+ bullshit_offset));
+                        TFC_SetMotorPWM(current_left_motor_speed-(.00005*num_of_straight), current_right_motor_speed+(.00005*num_of_straight)); // --left is faster, ++right is faster
+                        num_of_left = 0;
+                        num_of_right= 0;
+                        if (violence_level !=0)
+                            num_of_straight++;
+                    }
+                   
+                    else{}
+                    
+                    //dealwiththeshit
+                    // set servo and motors according to how much left we need to turn
+                    if(turn_left and (violence_level !=0))
+                    {
+                        num_of_straight = 0; // no longer on a straight
+                        num_of_right = 0;
+                        num_of_left++;
+                        turn_right = false;  
+                        if (left_counter + bullshit_offset > -.35)
+                            TFC_SetServo(0,left_counter + bullshit_offset ); // set turning servo
+                        
+                        // normalize to center each frame
+                        // left turning is - servo
+                        if(left_counter > -.2) // small turn, normalize quickly
+                            //left_counter += .015;    
+                            left_counter += .010;      
+                        else                // hard turn, normalize slowly
+                            left_counter += .009;
+                                
+                        // no longer turning boolean        
+                        if (left_counter > (0+ bullshit_offset))
+                            turn_left = false;
+
+                        if (need_decel) // need to deal with the decel
+                            {
+                                TFC_SetMotorPWM(current_left_motor_speed+(.5*left_counter), current_right_motor_speed-(.4*left_counter));  // ++left is slowed,--right is slowed 
+                                need_decel = false;
+                            }
+                        else  //  turning speeds 
+                            TFC_SetMotorPWM(current_left_motor_speed+(.25*left_counter), current_right_motor_speed+(.3*left_counter)+(.0001*num_of_left));  //  ++left is slowed, ++right is faster
+                        }// end of turn left
+                    
+                    // set servo and motors according to how much right we need to turn
+                    if(turn_right and (violence_level !=0))
+                    {
+                        num_of_straight = 0; // no longer going straight
+                        num_of_right++;
+                        num_of_left=0;
+                        
+                        turn_left =false;
+                        
+                        if (right_counter - bullshit_offset < .5)
+                            TFC_SetServo(0,right_counter - bullshit_offset); // set servo
+                        
+                        // normalize to center each frame
+                        // right turning is + servo
+                        if(right_counter < .2) // small turn, normalize quickly
+                            right_counter -= .017;        
+                        else                // hard turn, normalize slowly
+                            right_counter -= .01;
+                        
+                        
+                        // no longer turning boolean
+                        if (right_counter < (0+ bullshit_offset))
+                            turn_right = false;
+                            
+                        if(need_decel)// need to deal with the decel
+                            {
+                                TFC_SetMotorPWM(current_left_motor_speed+(.4*right_counter), current_right_motor_speed-(.5*right_counter));  // ++left is slowed,--right is slowed 
+                                need_decel = false;
+                            }
+                        else    //  turning speeds 
+                            TFC_SetMotorPWM(current_left_motor_speed-(.3*right_counter)-(.0001*num_of_right), current_right_motor_speed-(.25*right_counter));  //  --left is faster, --right is slowed      
+                        }   // end with turn right
+
+
+                    // clearing values for next image processing round
+                    black_value_count = 0;
+                    sum_black = 0;
+                    // 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 major loop
+}// end main
\ No newline at end of file