Diff: main.cpp

Revision:

100:43e306bf6f13

Parent:

98:d8ead5e04047

--- a/main.cpp	Thu Jun 13 11:12:37 2019 +0000
+++ b/main.cpp	Mon Jun 17 02:36:43 2019 +0000
@@ -28,34 +28,31 @@
 TRSensors trs;
 RawSerial pc(USBTX, USBRX, 115200);
 Adafruit_SSD1306_I2c gOled2(i2c,D9,0x7A,64,128);
-Ticker IRticker;
-Thread thread1;
-Thread thread2;
 Timer timer;
 PixelArray px(WS2812_BUF);
-
-// See the program page for information on the timing numbers
-// The given numbers are for the K64F
 WS2812 ws(D7, WS2812_BUF, 7, 15, 10, 15);
+// Thread thread1;
+// Thread thread2;
 
-// variables
+
 unsigned int sensorValues[button_SENSORS]; 
-unsigned int last_proportional = 0;
-long integral = 0;
+
+// Error variables
 static int prev_err = 0;
 static int int_err = 0;
+int err = 0;
+
+// PID & velocity value
 static float pval = 0.15;
 static float ival = 0.00003;
 static float dval = 2.3;
 static float vel = 120.0;
-uint8_t IR_buf[64];
-int length = 32;
+
 volatile int button = 0;
 int position = 0;
 int dist = 0;
 int cnt = 0;
-int err = 0;
-float t=0;
+float t = 0;
 
 // Reflective sensor
 bool rightObs= false;
@@ -132,6 +129,7 @@
 }
     int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
     int colorbuf3 =0x000000;
+    
 int main()
 {      
     for (int i = 0; i < WS2812_BUF; i++) {
@@ -317,38 +315,9 @@
                                 err = 0;
                                 prev_err = 0;
                                 break;
-                                
-                                
                             }
                         }
-                        
                     }
-                    //if(dist < 25)
-//                    {
-//                        int count=0;
-//                        while(count<100){
-//                            if(dist>10) break;
-//                            count++;
-//                        }
-//                        if(count<100) continue;
-//                        while(1)
-//                        {
-//                            
-//                            motorDriver.user_forward(0,0.2);
-//                            
-//                            //pc.printf("distance: %d\r\n", dist);
-//                            pc.printf("position: %d\r\n", position);
-//                            position=trs.readLine(sensorValues,0);
-//                            if(position > 2000)
-//                            {
-//                                int_err = 0;
-//                                err = 0;
-//                                prev_err = 0;
-//                                break;
-//                            }
-//                        }
-//                    }       
-                    
                     
                     err=(int)position-2000; // error>0 --> right, error<0 --> left
                     int_err+=err;
@@ -367,9 +336,9 @@
                         power_difference = -maximum;      
                         
                     if(power_difference<0)
-                        motorDriver.user_forward((maximum-10)/255,(maximum+power_difference)/255);               
+                        motorDriver.user_forward((maximum)/255,(maximum+power_difference)/255);               
                     else 
-                        motorDriver.user_forward((maximum-power_difference)/255,(maximum+10)/255); 
+                        motorDriver.user_forward((maximum-power_difference)/255,(maximum)/255); 
                     
                     pc.printf("position value: %d\r\n", position);