Diff: main.cpp

Revision:

95:250afd53b710

Parent:

88:bea4f2daa48c

Child:

96:7465ab270e7a

--- a/main.cpp	Thu Apr 11 15:00:04 2019 +0100
+++ b/main.cpp	Sat Jun 08 12:00:38 2019 +0000
@@ -1,32 +1,375 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2018 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
-
-#include "mbed.h"
-#include "stats_report.h"
-
-DigitalOut led1(LED1);
-
-#define SLEEP_TIME                  500 // (msec)
-#define PRINT_AFTER_N_LOOPS         20
-
-// main() runs in its own thread in the OS
-int main()
-{
-    SystemReport sys_state( SLEEP_TIME * PRINT_AFTER_N_LOOPS /* Loop delay time in ms */);
-
-    int count = 0;
-    while (true) {
-        // Blink LED and wait 0.5 seconds
-        led1 = !led1;
-        wait_ms(SLEEP_TIME);
-
-        if ((0 == count) || (PRINT_AFTER_N_LOOPS == count)) {
-            // Following the main thread wait, report on the current system status
-            sys_state.report_state();
-            count = 0;
-        }
-        ++count;
-    }
-}
+#include "TRSensors.h"
+#include "ReceiverIR.h"
+#include "Drive.h"
+#include "Adafruit_SSD1306.h"
+#include "WS2812.h"
+#include "PixelArray.h"
+#include "ultrasonic.h"
+
+#define NUM_SENSOR 5
+#define OLED_RESET D9
+#define WS2812_BUF 77   //number of LEDs in the array
+#define NUM_COLORS 6    //number of colors to store in the array
+#define NUM_STEPS 8    //number of steps between colors
+
+#define PCF8574_ADDR 0x20
+#define beep_on  PCF8574Write(0xDF & PCF8574Read())
+#define beep_off PCF8574Write(0x20 | PCF8574Read())
+
+// IR REMOTE
+ReceiverIR ir_rx(PB_5);
+RemoteIR::Format format;
+uint8_t buf[8];
+int bitlength;
+
+// Control motor drive
+Drive drive(D6, D5, A1, A0, A2, A3);
+int control_speed = 2500;
+int calibrate_speed = 4000;
+int running_speed = 4000;
+
+// TR Sensor
+TRSensors trs = TRSensors(D10, D11, D12, D13);
+uint16_t sensorValues[NUM_SENSOR];
+unsigned int last_proportional;
+long integral;
+int last_power_difference = 0;
+
+// IR sensor
+I2C i2c_for_irsensor(PB_9, PB_8);
+
+// Ultra sonic sensor
+Ultrasonic us(D3, D2, 0.1, false, 0.1);
+
+// OLED time print
+I2C i2c_for_oled(I2C_SDA,I2C_SCL);
+Adafruit_SSD1306_I2c myOled(i2c_for_oled,OLED_RESET,0x78,64,128);
+Timer t;
+
+// LED when in goal
+PixelArray px(WS2812_BUF);
+WS2812 ws(D7, WS2812_BUF, 6,17,9,14);   //nucleo-f411re // See the program page for information on the timing numbers
+
+// Debug
+RawSerial pc(PA_2, PA_3, 115200);
+
+int receive(RemoteIR::Format *format, uint8_t *buf, int bufsiz, int timeout = 100) {
+    int cnt = 0;
+    while (ir_rx.getState() != ReceiverIR::Received) {
+        cnt++;
+        if (timeout < cnt) {
+            return -1;
+        }
+    }
+    return ir_rx.getData(format, buf, bufsiz * 8);
+}
+
+void calibration(){
+    memset(buf, 0x00, sizeof(buf));
+    drive.setSpeed(calibrate_speed);
+    while(true)
+    {
+        bitlength = receive(&format, buf, sizeof(buf));
+        if (bitlength < 0) {
+            continue;
+        }
+        
+        if(buf[2] == 0x18)
+            drive.Forward();
+                
+        else if(buf[2] == 0x52)
+            drive.Backward();
+            
+        else if(buf[2] == 0x8)
+            drive.Turn_left();
+                
+        else if(buf[2] == 0x5A)
+            drive.Turn_right();
+            
+        else if(buf[2] == 0x1C)
+            drive.Break();
+            
+        else if(buf[2] == 0x0C)
+        {
+            drive.Break();
+            break;
+        }
+        
+        wait(0.1);
+        drive.Break();
+        pc.printf("calibration!\r\n");
+                
+        trs.calibrate();
+    }
+}
+
+void ready_to_drive(){
+    drive.setSpeed(control_speed);
+    memset(buf, 0x00, sizeof(buf));
+    while(true){
+        bitlength = receive(&format, buf, sizeof(buf));
+        if (bitlength < 0) {
+            continue;
+        }
+        
+        if(buf[2] == 0x18)
+            drive.Forward();
+                
+        else if(buf[2] == 0x52)
+            drive.Backward();
+            
+        else if(buf[2] == 0x8)
+            drive.Turn_left();
+                
+        else if(buf[2] == 0x5A)
+            drive.Turn_right();
+            
+        else if(buf[2] == 0x1C)
+            drive.Break();
+            
+        else if(buf[2] == 0x5E)
+        {
+            drive.Break();
+            break;
+        }
+        
+        wait(0.1);
+        drive.Break();
+    }
+}
+
+void PCF8574Write(char data){
+    i2c_for_irsensor.write((PCF8574_ADDR<<1), &data, 1);
+}
+
+void PCF8574Initialize(){
+    PCF8574Write(0x01);
+}
+
+char PCF8574Read(){
+    char data = 0xFF;
+    i2c_for_irsensor.read(((PCF8574_ADDR<<1)|0x01), &data, 1);
+    return data;
+}
+
+void check_irsensor(void){
+    char data;
+    
+    PCF8574Write(0xC0 | PCF8574Read());
+    data = PCF8574Read() | 0x3F;
+    if(data == 0x7F){
+        pc.printf("left\r\n");
+        // avoid_right_obstacle();
+    } else if(data == 0xBF){
+        pc.printf("right\r\n");
+        // avoid_left_obstacle();
+    }
+}
+
+void check_obstacle_with_ultrasonic_sensor(void){
+    if(us.getDistance()< 10){ // Reduce magic number
+        beep_on;
+    } else {
+        beep_off;
+    }
+}
+
+void start_auto_drive(){
+    drive.setSpeed(running_speed);
+//    drive.Forward();
+    while(true){
+        check_irsensor();
+        check_obstacle_with_ultrasonic_sensor();
+        
+        unsigned int position = trs.readLine(sensorValues);
+        
+//        // for pc debug
+//        for(int i=0; i<5; i++){
+//            pc.printf("%d\t", sensorValues[i]);
+//        }
+//        pc.printf("\r\n");
+//        
+//        // for oled debug
+//        for(int i=0; i<5; i++){
+//            myOled.printf("%d\t", sensorValues[i]);
+//            myOled.display();
+//        }
+//        myOled.printf("\r");
+//        myOled.display();
+        
+        // It need to have a specific rule
+        if( (sensorValues[0] >= 800) &&
+            (sensorValues[1] >= 800) && 
+            (sensorValues[2] >= 800) &&
+            (sensorValues[3] >= 800) &&
+            (sensorValues[4] >= 800) )
+        {
+            drive.Break();
+            break;
+        }
+        
+        if(sensorValues[0] >= trs.calibratedMax[0]-15 && sensorValues[0] <= trs.calibratedMax[0]+15)
+        {
+            drive.setSpeed(running_speed-2800, running_speed);
+//            drive.Forward();
+        }
+        
+        else if(sensorValues[1] >= trs.calibratedMax[1]-15 && sensorValues[1] <= trs.calibratedMax[1]+15)
+        {
+            drive.setSpeed(running_speed-700, running_speed);
+//            drive.Forward();
+        }
+        
+        else if(sensorValues[2] >= trs.calibratedMax[2]-15 && sensorValues[2] <= trs.calibratedMax[2]+15)
+        {
+            drive.setSpeed(running_speed, running_speed);
+//            drive.Forward();
+        }
+        
+        else if(sensorValues[3] >= trs.calibratedMax[3]-15 && sensorValues[3] <= trs.calibratedMax[3]+15)
+        {
+            drive.setSpeed(running_speed, running_speed-700);
+//            drive.Forward();
+        }
+        
+        else if(sensorValues[4] >= trs.calibratedMax[4]-15 && sensorValues[4] <= trs.calibratedMax[4]+15)
+        {
+            drive.setSpeed(running_speed, running_speed-2800);
+//            drive.Forward();
+        }
+        else {
+//            drive.Forward();
+        }
+        
+        wait(0.1);
+        drive.Break();
+    }
+}
+
+
+int color_set(uint8_t red,uint8_t green, uint8_t blue)
+{
+  return ((red<<16) + (green<<8) + blue);   
+}
+
+// 0 <= stepNumber <= lastStepNumber
+int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
+{
+    return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
+}
+
+
+void bottom_led(void){  //led 제어 
+     int colorIdx = 0;
+    int colorTo = 0;
+    int colorFrom = 0;
+    
+    uint8_t ir = 0;
+    uint8_t ig = 0;
+    uint8_t ib = 0;
+    
+       
+    ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
+    
+    // set up the colours we want to draw with
+    int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
+    
+    // Now the buffer is written, write it to the led array.
+    while (1) 
+    {
+        //get starting RGB components for interpolation
+        std::size_t c1 = colorbuf[colorFrom];
+        std::size_t r1 = (c1 & 0xff0000) >> 16;
+        std::size_t g1 = (c1 & 0x00ff00) >> 8;
+        std::size_t b1 = (c1 & 0x0000ff);
+        
+        //get ending RGB components for interpolation
+        std::size_t c2 = colorbuf[colorTo];
+        std::size_t r2 = (c2 & 0xff0000) >> 16;
+        std::size_t g2 = (c2 & 0x00ff00) >> 8;
+        std::size_t b2 = (c2 & 0x0000ff);
+        
+        for (int i = 0; i <= NUM_STEPS; i++)
+        {
+            ir = interpolate(r1, r2, i, NUM_STEPS);
+            ig = interpolate(g1, g2, i, NUM_STEPS);
+            ib = interpolate(b1, b2, i, NUM_STEPS);
+            
+            //write the color value for each pixel
+            px.SetAll(color_set(ir,ig,ib));
+            
+            //write the II value for each pixel
+            px.SetAllI(32);
+            
+            for (int i = WS2812_BUF; i >= 0; i--) 
+            {
+                ws.write(px.getBuf());
+            }
+        }
+        
+        colorFrom = colorIdx;
+        colorIdx++;
+        
+        if (colorIdx >= NUM_COLORS)
+        {
+            colorIdx = 0;
+        }
+        
+        colorTo = colorIdx;
+        
+    }
+}
+
+int main(){
+    //program setup
+    myOled.begin();
+    i2c_for_irsensor.frequency(100000);    // main에 써야함
+    
+    // Test OLED
+    myOled.printf("oled size: %ux%u \r\nStart Alphabot2\r\n", myOled.width(), myOled.height());
+    myOled.display();
+    
+    //program start
+    memset(buf, 0x00, sizeof(buf));
+    
+    while(true){
+        bitlength = receive(&format, buf, sizeof(buf));
+        if (bitlength < 0) {
+            continue;
+        }
+        
+        if(buf[2] == 0x42)
+            break;
+    }
+    
+    //calibration
+//    pc.printf("calibration!\r\n");
+    myOled.printf("calibration!\r");
+    myOled.display();
+    calibration();
+
+    //ready autodriving
+//    pc.printf("ready_to_drive!\r\n");
+    myOled.printf("ready_to_drive!\r");
+    myOled.display();
+    ready_to_drive();
+
+    //go autodrive
+//    pc.printf("start_auto_drive!\r\n");
+    myOled.printf("start_auto_drive!\r\n");
+    myOled.display();
+    t.start();
+    start_auto_drive();
+    t.stop();
+    beep_off;
+    pc.printf("The time taken was %f seconds\r\n", t.read());
+    
+    /* OLED time print */
+    myOled.printf("\nGoal! \r\nThe time taken was\r\n%f seconds\r\n", t.read());
+    myOled.display();
+    /* LED run*/ 
+    bottom_led();
+    
+    while(true){}
+}
+