Diff: main.cpp

Revision:

1:12e11042b2e9

Parent:

0:39bb015c8ac2

Child:

2:fe723d003d1f

--- a/main.cpp	Thu Oct 29 04:25:23 2015 +0000
+++ b/main.cpp	Thu Oct 29 19:26:23 2015 +0000
@@ -281,6 +281,38 @@
 	}
 }
 
+void run_4500_R()
+{
+
+	float current;
+	int output;
+	MOT_B = 0.0;
+	MOT_C = 0;
+	MOT_A = 1;
+	current = 0.0;
+	for(int i = 0; i < 10; i++)
+	{
+		current = current + CSENSE.read();
+		//wait_us(10);
+	}
+	current = current/10;
+	current  = ((current*3.3f)/25.0f)/(0.015f);
+	current = current*1000.0f;
+	output = static_cast<int>(current);
+	//pc.printf("Motor Current = %d mA\r\n",output );
+
+	//wait(0.001);
+	if(output >= 4500)
+	{
+		MOT_D = MOT_D - (0.01f*(((output-4500)/1)));
+	}
+	if(output < 4500)
+	{
+		MOT_D = 1.0f;
+	}
+}
+
+
 
 int main()
 {
@@ -314,14 +346,14 @@
             sleep();
             //deepsleep();
             wait(0.1);
-            SEL1=1;
+            SEL1=0;
             SEL2=1;
             CSENSE_EN = 1;
         } else {
             //printf("%d: Running\n", i);
             myled = 1;
             Orange = 1;  
-            run_1500_R();
+            run_4500_R();
             
         }
         i++;