Diff: main.cpp

Revision:

6:44d1079f076c

Parent:

5:e7e4f1967fb8

Child:

7:455e7dd338ee

--- a/main.cpp	Wed Feb 18 19:54:07 2015 +0000
+++ b/main.cpp	Wed Feb 18 20:51:28 2015 +0000
@@ -8,6 +8,9 @@
     TFC_Init();
     
     float current_servo_position = 0;
+    float current_left_motor_speed = 0;
+    float current_right_motor_speed = 0;
+    bool rear_motor_enable_flag = true;
     
     while(1) 
     {
@@ -15,14 +18,14 @@
         if (TFC_ReadPushButton(0) != 0 )
             {
                 TFC_BAT_LED0_ON;
-                wait(0.2);
+                wait(0.004);
                 TFC_BAT_LED0_OFF; 
-                wait(0.2);
+                wait(0.004);
                 
-                current_servo_position = current_servo_position-.05;
+                current_servo_position = current_servo_position-.005;
+                if(current_servo_position <= -0.4)
+                    current_servo_position = -0.4;
                 TFC_SetServo(0, current_servo_position);
-                
-                
             }
             
         else
@@ -33,11 +36,13 @@
         if (TFC_ReadPushButton(1) != 0 )
             {
                 TFC_BAT_LED1_ON;
-                wait(0.2);
+                wait(0.004);
                 TFC_BAT_LED1_OFF; 
-                wait(0.2);
+                wait(0.004);
                 
-                current_servo_position = current_servo_position+.05;
+                current_servo_position = current_servo_position+.005;
+                if(current_servo_position >= 0.4)
+                    current_servo_position = 0.4;
                 TFC_SetServo(0, current_servo_position);
             }
             
@@ -46,6 +51,26 @@
                 TFC_BAT_LED1_ON;
             }
             
+        if(rear_motor_enable_flag)
+            {
+                //
+                //current_left_motor_speed    = current_left_motor_speed +  .3*(TFC_ReadPot(0));
+                //current_right_motor_speed   = current_right_motor_speed +  .3*(TFC_ReadPot(1));
+                
+                current_left_motor_speed    = .3*(TFC_ReadPot(0));
+                current_right_motor_speed   = .3*(TFC_ReadPot(1));
+                
+                if(current_left_motor_speed >= 0.4)
+                    current_left_motor_speed= 0.4;
+                if(current_right_motor_speed >= 0.4)
+                    current_right_motor_speed= 0.4;    
+                if(current_left_motor_speed <= -0.4)
+                    current_left_motor_speed= -0.4;
+                if(current_right_motor_speed <= -0.4)
+                    current_right_motor_speed= -0.4;                       
+                    
+                TFC_SetMotorPWM(current_left_motor_speed, current_right_motor_speed);
+            }    
     }
 }
 //hi guys
\ No newline at end of file