Diff: main.cpp

Revision:

16:11ba5d6f42ba

Parent:

15:830209e846d5

Child:

17:c643b6b4a96f

Child:

18:3988310cd03b

--- a/main.cpp	Wed Feb 25 16:05:21 2015 +0000
+++ b/main.cpp	Wed Feb 25 17:56:16 2015 +0000
@@ -59,12 +59,12 @@
             violence_level =  int(TFC_GetDIP_Switch());
 
             if (violence_level==2) {
-                current_left_motor_speed  = .28;
-                current_right_motor_speed = .28;
+                current_left_motor_speed  = -.48;
+                current_right_motor_speed = .48;
             }
             else if (violence_level==1) {
-                current_left_motor_speed  = .2;
-                current_right_motor_speed = .2;
+                current_left_motor_speed  = -.35;
+                current_right_motor_speed = .35;
             }
             else if (violence_level==0) {
                 current_left_motor_speed  = 0;
@@ -131,15 +131,29 @@
                     // need to turn left
                     if (black_center_value < 64) {
                         
-                        current_servo_position= float(.015625*black_center_value-(1));
+                        current_servo_position= float(.01875*black_center_value-(1.2));
                         if(current_servo_position <= -0.4)
                             current_servo_position = -0.4;
                         TFC_SetServo(0, current_servo_position);
 
 
-                        current_left_motor_speed  = current_left_motor_speed  - float(64-black_center_value)*.0025;
-                        current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
-
+                        //current_left_motor_speed  = current_left_motor_speed  + float(64-black_center_value)*.0025;
+                        //current_right_motor_speed = current_right_motor_speed + float(64-black_center_value)*.0025;
+                        if (violence_level !=0){
+                        current_left_motor_speed  = current_left_motor_speed  + float(float(64-black_center_value)*.045);// kinda reverse this...
+                        current_right_motor_speed = current_right_motor_speed + float(float(64-black_center_value)*.045);// push more forwards
+                        }
+                        
+                        // protection block
+                        if(current_left_motor_speed >= 0.5)
+                            current_left_motor_speed= 0.5;
+                        if(current_right_motor_speed >= 0.5)
+                            current_right_motor_speed= 0.5;
+                        if(current_left_motor_speed <= -0.5)
+                            current_left_motor_speed= -0.5;
+                        if(current_right_motor_speed <= -0.5)
+                            current_right_motor_speed= -0.5;
+                        
                         TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
 
                     }
@@ -147,14 +161,28 @@
                     // need to turn right
                     if (black_center_value > 64) {
 
-                        current_servo_position= float(.015625*black_center_value-(1));
+                        current_servo_position= float(.01875*black_center_value-(1.2));
                         if( current_servo_position >= +0.4)
                             current_servo_position = +0.4;
                         TFC_SetServo(0, current_servo_position);
 
-                        current_left_motor_speed  = current_left_motor_speed  + float(black_center_value-64)*.0025;
-                        current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
+                        //current_left_motor_speed  = current_left_motor_speed  - float(black_center_value-64)*.0025;
+                        //current_right_motor_speed = current_right_motor_speed - float(black_center_value-64)*.0025;
+                        if (violence_level !=0){
+                        current_left_motor_speed  = current_left_motor_speed  - float(float(black_center_value-64)*.045);// push more forwards
+                        current_right_motor_speed = current_right_motor_speed - float(float(black_center_value-64)*.045);// kinda reverse this...
+                        }
 
+                        // protection block
+                        if(current_left_motor_speed >= 0.5)
+                            current_left_motor_speed= 0.5;
+                        if(current_right_motor_speed >= 0.5)
+                            current_right_motor_speed= 0.5;
+                        if(current_left_motor_speed <= -0.5)
+                            current_left_motor_speed= -0.5;
+                        if(current_right_motor_speed <= -0.5)
+                            current_right_motor_speed= -0.5;
+                            
                         TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
 
                     }