Diff: main.cpp

Revision:

0:466ee63955df

Child:

2:2439f5a4a93d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 14 09:09:52 2016 +0000
@@ -0,0 +1,79 @@
+/*********************************************************
+*RenBuggyLineFollower                                    *
+*Author: Dan Argust                                      *
+*                                                        *  
+*This program demonstates the use of two sensors to      *
+*detect and follow a thin black line.                    *
+*********************************************************/
+
+#include "mbed.h" //"mbed.h" is a library that makes it easier to program microcontrollers
+#include "USBSerial.h"
+//#include "TimedMovement.h" //"TimedMovement.h" contains the functions that we will use to move the buggy
+
+AnalogIn ain0(p15);
+AnalogIn ain1(p16);
+
+PwmOut pwm0(p25);
+PwmOut pwm1(p10);
+
+float Lmotor;
+float Rmotor;
+float newLine;
+float oldLine;
+
+//USBSerial USB;
+Ticker timer;
+
+void getMotorSpeeds(float a,float b){
+    float inputs[2] = {a,b};
+    float max_speed = 1.0;
+    Lmotor = 0;
+    Rmotor = 0;
+    newLine = 0;
+    float line_to_speed_factor = 5;
+    for(int i = 0;i<2;++i){
+        newLine += inputs[i]*(i+1);
+    }
+    float sum = 0;
+    for (int i = 0;i<2;++i){
+        sum += inputs[i];
+    }
+    newLine = newLine / sum;
+    if((a<0.2)&&(b<0.2)){
+        if(oldLine>1.5)
+            newLine = 2;
+        else
+            newLine = 1;
+    }
+    oldLine = newLine;
+    /*USB.printf("\033[2J");
+    USB.printf("\033[0;0H");
+    USB.printf("input a = %f\n\rinput b = %f\n\r",a,b);
+    USB.printf("%f\n",line);*/
+    Lmotor = newLine*line_to_speed_factor-6.5;
+    Rmotor = 2-(newLine*line_to_speed_factor-6.5);
+    if(Lmotor>max_speed){
+        Lmotor = max_speed;
+    }
+    if(Rmotor>max_speed){
+        Rmotor = max_speed;
+    }
+}
+
+/*void PrintOutputs(){
+    USB.printf("\033[2J");
+    USB.printf("\033[0;0H");
+    USB.printf("Left  : %f\n\r",Lmotor);
+    USB.printf("Right : %f\n\r",Rmotor);
+}*/
+
+int main()
+{
+    //timer.attach(&PrintOutputs,0.1);
+    while(true){
+        getMotorSpeeds(ain0.read(),ain1.read());
+        pwm0 = Lmotor;
+        pwm1 = Rmotor;
+        wait_ms(10);
+    }
+}
\ No newline at end of file