Diff: main.cpp

Revision:

98:d8ead5e04047

Parent:

97:8f3abd5cf5ce

Child:

99:6efb1c0a6a68

Child:

100:43e306bf6f13

--- a/main.cpp	Wed Jun 12 08:20:53 2019 +0000
+++ b/main.cpp	Thu Jun 13 11:12:37 2019 +0000
@@ -5,12 +5,18 @@
 #include "TRSensors.h"
 #include "ultrasonic.h"
 #include "Adafruit_SSD1306.h"
+#include "WS2812.h"
+#include "PixelArray.h"
 
 #define button_SENSORS 5
 #define ADT7420_TEMP_REG (0x00)
 #define ADT7420_CONF_REG (0x03)
 #define EVAL_ADT7420_ADDR (0x48)
 #define PCF8574_ADDR (0x20)
+#define WS2812_BUF 100
+#define WS2812_BUF2 4
+#define NUM_COLORS 3
+#define NUM_LEDS_PER_COLOR 4
 
 // create object
 DigitalOut dc(D8,1);
@@ -26,6 +32,11 @@
 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);
 
 // variables
 unsigned int sensorValues[button_SENSORS]; 
@@ -33,10 +44,10 @@
 long integral = 0;
 static int prev_err = 0;
 static int int_err = 0;
-static float pval = 0;
-static float ival = 0;
-static float dval = 2.0;
-static float vel = 150.0;
+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;
@@ -57,17 +68,17 @@
 float fPwmBPulsewidth;
 
 // opearte ultrasonic
-/*void ultrasonic_thread()
-{
-    while (1)
-    {
-        ultra.setMode(false);           // have updates every .1 seconds and try only once.
-        ultra.trig();
-        dist = ultra.getDistance();
-        pc.printf("Detected value: %d\r\n", dist);
-    }
-}
-*/
+//void ultrasonic_thread()
+//{
+//    while (1)
+//    {
+//        ultra.setMode(false);           // have updates every .1 seconds and try only once.
+//        ultra.trig();
+//        dist = ultra.getDistance();
+//        pc.printf("Detected value: %d\r\n", dist);
+//    }
+//}
+
 
 // opearte reflective sensors
 /*
@@ -119,9 +130,17 @@
                 gOled2.printf("D: %.2f\r\n",dval);
                 gOled2.display();
 }
-
+    int colorbuf[NUM_COLORS] = {0xff0000,0x00ff00,0x0000ff};
+    int colorbuf3 =0x000000;
 int main()
-{   
+{      
+    for (int i = 0; i < WS2812_BUF; i++) {
+        px.Set(i, colorbuf[(i / NUM_LEDS_PER_COLOR) % NUM_COLORS]);
+    }
+    for (int j=0; j<WS2812_BUF; j++) {
+        // px.SetI(pixel position, II value)
+        px.SetI(j%WS2812_BUF, 0xf+(0xf*4));
+    }
     
  //   thread1.start(ultrasonic_thread);
  //   thread2.start(reflective_thread);
@@ -238,6 +257,7 @@
             case 0x18:
             // 2 button (move forward)
                 motorDriver.user_forward(0.3,0.3);
+                trs.calibrate();
                 wait(0.1);
                 button = 0x1C;
                 break;
@@ -245,6 +265,7 @@
             case 0x52:
             // 8 button (move backward)
                 motorDriver.user_backward(0.3,0.3);
+                trs.calibrate();
                 wait(0.1);
                 button = 0x1C;
                 break;
@@ -273,28 +294,22 @@
                 t=0;
                 timer.reset();
                 timer.start();
-                
+                ultra.setMode(false);           // have updates every .1 seconds and try only once.
+
                 while(1)
                 {  
                     t=timer.read();
-                    update_display();
                     position=trs.readLine(sensorValues,0);
-                    
-                    /*if(dist < 10)
-                    {
-                        int count=0;
-                        while(count<150){
-                            if(dist>10) break;
-                            count++;
-                        }
-                        if(count<150) continue;
-                        while(1)
-                        {
-                            
-                            motorDriver.user_left(0.2,0.2);
-                            
-                            //pc.printf("distance: %d\r\n", dist);
-                            pc.printf("position: %d\r\n", position);
+                    ultra.trig();
+                    dist = ultra.getDistance();
+                    pc.printf("dist:%d\r\n", dist);
+                    if(dist<=21){
+                          motorDriver.user_left(0.2,0.2);
+                          wait(0.1);
+                          while(1){
+                           
+                           // pc.printf("distance: %d\r\n", dist);
+                           // pc.printf("position: %d\r\n", position);
                             position=trs.readLine(sensorValues,0);
                             if(position > 2000)
                             {
@@ -302,9 +317,37 @@
                                 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
@@ -324,9 +367,9 @@
                         power_difference = -maximum;      
                         
                     if(power_difference<0)
-                        motorDriver.user_forward((maximum+15)/255,(maximum+power_difference)/255);               
+                        motorDriver.user_forward((maximum-10)/255,(maximum+power_difference)/255);               
                     else 
-                        motorDriver.user_forward((maximum-power_difference)/255,(maximum-15)/255); 
+                        motorDriver.user_forward((maximum-power_difference)/255,(maximum+10)/255); 
                     
                     pc.printf("position value: %d\r\n", position);
                     
@@ -339,6 +382,12 @@
                         timer.stop();
                         t=timer.read();
                         update_display();
+                        motorDriver.stop();
+                        for(int z=48;z>=0;z=z-4){
+                        ws.write_offsets(px.getBuf(),z,z,z);
+                        wait(0.1);
+                        }
+                        
                         button = 0x1C;
                         break;
                     }
@@ -347,15 +396,14 @@
                 
             case 0x42:
             // 7 button (read sensor values)
-                while(1)
-                {
-                    position=trs.readLine(sensorValues,0);
-                    for(int i=0;i<5;i++){
-                        pc.printf("%d\r\n",sensorValues[i]);
-                    }
-                    pc.printf("done!\r\n");
-                    wait(1);
+                position=trs.readLine(sensorValues,0);
+                for(int i=0; i<5; i++) {
+                    pc.printf("%d\r\n",sensorValues[i]);
                 }
+                pc.printf("done!\r\n");
+                button = 0x1C;
+                break;
+
                 
             case 0x4A:
             // 9 button (read position)
@@ -368,7 +416,7 @@
                 } 
                 button = 0x1C;
                 break;                
-            dafault:
+            default:
                 // wrong button
                 pc.printf("wrong button!\r\n");
                 break;