Diff: main.cpp

Revision:

97:b483e656bd14

Parent:

88:bea4f2daa48c

--- a/main.cpp	Fri May 31 13:00:04 2019 +0100
+++ b/main.cpp	Sun Jun 16 04:44:35 2019 +0000
@@ -1,32 +1,432 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2018 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
+#include "mbed.h"
+#include <iostream>
+#include "HCSR04.h"
+#include <list>
+#include <string.h>
+#include <IRremote.h>
+#include "TB6612FNG.h"
+//#define PCF8574_ADDR     (0x40) 
+#define LEFT 127
+#define RIGHT 191
+#define NUMSENSORS 5
+#define MAX 1023
+#define MIN 0
+#define TRU 1
+#define FALS 0
+#define MaxSpeed 255// max speed of the robot
+#define BaseSpeed 155 
+#define speedt 180
+
+#include "SSD1306-Library.h"
+#include "FreeSans9pt7b.h"
+#include <time.h>
+#include <math.h>
+
+#define KEY2 0xFF18E7                 //Key:2 
+#define KEY8 0xFF52AD                 //Key:8 
+#define KEY4 0xFF08F7                 //Key:4 
+#define KEY6 0xFF5AA5                 //Key:6 
+#define KEY1 0xFF0CF3                 //Key:1 
+#define KEY3 0xFF5EA1                 //Key:3 
+#define KEY5 0xFF1CE3                 //Key:5
+
+
+//I2C i2c(I2C_SDA, I2C_SCL);  //D15,D14
+SPI spi(D11, D12, D13);
+DigitalOut cs(D10,1);
+Serial pc(PA_2, PA_3, 115200); //D1 D0
+IRrecv irrecv(D4);
+
+
+int mode=1;
+int index=0;
+int rx_buffer[10];
+char arr[16];
+volatile int flag = 0;
+float interval=0.1; //100ms
+decode_results result;
+
+
+
+/////////sensors////////////
+
+int temp=0;
+int calibratedMin[NUMSENSORS];
+int calibratedMax[NUMSENSORS];
+unsigned int sensor_values[NUMSENSORS];
+int in_line[NUMSENSORS];
+int ultra_flag=0;
+float position;
+
+/////////sensors////////////
+
+/////////Ultrasonic////////////
+Ultrasonic ultra(D3, D2);
+int ultra_result=10;
+/////////Ultrasonic////////////
+
+
+//////////motor////////////
+PwmOut      motor_pwmA(PB_10); //D6
+PwmOut      motor_pwmb(PB_4);  // D5
+AnalogIn rightMotor2(A0);
+AnalogIn rightMotor1(A1);
+AnalogIn leftMotor2(A3);
+AnalogIn leftMotor1(A2);
+
+TB6612FNG leftmotor(PB_10, A1, A0);
+TB6612FNG rightmotor(PB_4, A2, A3);
+float last_proportional;
+float integral;
+float kp = 0.5;
+float kd = 0.1;
+int lastError;
+//////////motor////////////
+
+void IRForward(void)
+{
+    leftmotor.setSpeed(0.1);
+    rightmotor.setSpeed(0.1);
+
+}
+
+void ultraLeft(){
+    leftmotor.setSpeed(-0.13);
+    rightmotor.setSpeed(0.15);
+}
+
+void left(void)
+{
+    leftmotor.setSpeed(0.03);
+    rightmotor.setSpeed(0.15);
+}
+
+
+void right(void)
+{
+    leftmotor.setSpeed(0.15);
+    rightmotor.setSpeed(0.03);
+}
+
+void stop(void)
+{
+    leftmotor.setSpeed(0);
+    rightmotor.setSpeed(0);
+}
+
+void CalibrationMotor(void)
+{
+    leftmotor.setSpeed(0.15);
+    rightmotor.setSpeed(-0.15);
+}
+
+void backward(void)
+{
+   leftmotor.setSpeed(-0.1);
+   rightmotor.setSpeed(-0.1);
+}
+
 
-#include "mbed.h"
-#include "stats_report.h"
+//bool pcf8574_write(uint8_t data){
+//    return i2c.write(PCF8574_ADDR, (char*) &data, 1, 0) == 0;
+//}
+//
+//bool pcf8574_read(uint8_t* data){
+//    return i2c.read(PCF8574_ADDR, (char*) data, 1, 0) == 0;
+//}
+//
+//int pcf8574_test(uint8_t value){
+//    
+//    int ret;
+//    uint8_t data=0;
+//    
+//    ret = pcf8574_write(value);
+//    if(!ret) return -1;
+//    
+//    ret = pcf8574_read(&data);
+//    if(!ret) return -2;
+//    
+//    return data;
+//}
+//
+//void detected(void)
+//{
+//    pcf8574_test(0xff);
+//    while(pcf8574_test(0xff)<255) pcf8574_test(0x00);
+//}
+
+int state=0; //0:forward / 1:left / 2:right
+int cnt=0;
+ int period_us;
+    int beat_ms;
 
-DigitalOut led1(LED1);
+void TRSensors(void) {
+    
+    for(int i=0; i< NUMSENSORS ; i++) {
+            calibratedMin[i]= MAX;
+            calibratedMax[i] = MIN;
+    }
+        
+}
+
+void set_TLC1543 (void) {
+   
+   int value;
+   for(int i=0; i<6 ; i++){
+    cs=0;
+    wait_us(2);
+    value=spi.write(i<<12);
+    cs=1;
+    wait_us(21);
+    cs=0;
+    wait_us(2);
+    value=spi.write(i<<12);
+    //pc.printf("%d th : 0x%X\r\n", i, value);
+    cs=1;
+    wait_us(21);
+    sensor_values[i]=value;
+    }   
+}
+
+void calibrate(void){
+    int i;
+
+    unsigned int max_sensor_values[NUMSENSORS];
+    unsigned int min_sensor_values[NUMSENSORS];   
+    
+    int j;
+    for(j=0; j <10 ; j++){
+         set_TLC1543();
+         for(i=0; i<NUMSENSORS; i++){
+         if(j==0 || max_sensor_values[i] < sensor_values[i]) max_sensor_values[i] = sensor_values[i];
+         if(j==0 || min_sensor_values[i] > sensor_values[i]) min_sensor_values[i] = sensor_values[i];
+         }
+    }
+    
+    for(i=0; i<NUMSENSORS; i++){
+        if(min_sensor_values[i] > calibratedMax[i]) calibratedMax[i] = min_sensor_values[i];
+        if(max_sensor_values[i] < calibratedMin[i]) calibratedMin[i] = max_sensor_values[i];
+    }
+
+}
 
-#define SLEEP_TIME                  500 // (msec)
-#define PRINT_AFTER_N_LOOPS         20
+void readCalibrated(){
+    int i;
+    set_TLC1543();
+    for(i=0; i<NUMSENSORS ; i++){
+        //unsigned int calmin, calmax;
+        unsigned int denominator;
+        denominator = calibratedMax[i] - calibratedMin[i];
+        signed int x =0;
+        if(denominator !=0) {
+            x = (   (     (signed long)sensor_values[i]  ) -calibratedMin[i]) * 1000 / denominator;    
+        }
+        if(x<0) x=0;
+        else if(x>1000) x= 1000;
+        else sensor_values[i] = x;
+    }    
+}
 
-// main() runs in its own thread in the OS
+int readLine() {
+    unsigned char i, on_line =0;
+    unsigned long avg;
+    unsigned int sum;
+    static int last_value=0;
+    
+    readCalibrated();
+    avg =0; sum =0;
+    for(i=0; i< NUMSENSORS; i++){
+        int value = sensor_values[i];
+        value = 1000-value;
+        sensor_values[i]=value;
+        in_line[i]=FALS;
+        if(value > 300) {
+            on_line =1;
+            in_line[i]=TRU;
+        }  
+        //if(value > calibratedMin[i]) on_line=1;
+        if(value > 50){
+        avg += (long)(value)*(i*1000);
+        sum += value;    
+        }         
+    }
+    
+    if(!on_line){
+        if(last_value < (NUMSENSORS-1)*1000/2) return 0;
+        else return ( NUMSENSORS-1 ) * 1000;   
+    }
+   
+   last_value = avg/sum;
+   
+   return last_value;
+            
+}
+
+/****************************************************************
+before running, set up tlc1543, calibrate **********************/
+void settlc1543(){
+    cs =1;
+    spi.format(16,0);
+    spi.frequency(2000000);
+    pc.printf("test\r\n");
+    //pc.attach(callback(&rx_cb));
+    
+    TRSensors();
+    for(int i=0; i<10; i++) {
+        calibrate();
+        wait(0.4);
+    }
+//    pc.printf("calibrate done\r\n");
+    for(int i=0; i<NUMSENSORS; i++) {
+//        pc.printf("Calibration Min: %d Max: %d\r\n", calibratedMin[i], calibratedMax[i] );
+    }
+}
+
+
+void rx_cb(void) {
+  
+    int ch;
+    ch = pc.getc();
+    rx_buffer[index]=ch;
+    index++;
+    pc.putc(ch); 
+   if(ch==0x0D) {
+    pc.putc(0x0A);
+    flag=1;
+    index=0;
+    }
+
+    
+}
+
+
 int main()
 {
-    SystemReport sys_state( SLEEP_TIME * PRINT_AFTER_N_LOOPS /* Loop delay time in ms */);
+    clock_t t;
+    int f;
+    char displaySentence[40] = "Time is ";
+    char time[20];
+    irrecv.enableIRIn();
+//    mu.startUpdates();//start mesuring the distance
+    int data;
+    
+    SSD1306 display = SSD1306();
+    
+    while(1){
+            
+            if(irrecv.decode(&result)){
+            pc.printf("%X\r\n",result.value);
+            irrecv.resume();
+            
+            if(result.value == KEY1){       // 1
+                break;
+            }
+            if(result.value == KEY2){       // 2
+               IRForward();
+            }            
+            if(result.value == KEY3){       // 3
+                CalibrationMotor();
+                settlc1543();
+                
+            }
+            if(result.value == KEY6){       // 6
+               right();
 
-    int count = 0;
-    while (true) {
-        // Blink LED and wait 0.5 seconds
-        led1 = !led1;
-        wait_ms(SLEEP_TIME);
+            }
+            if(result.value == KEY4){       // 4
+               left();
+            }
+            if(result.value == KEY8 ){       // 8
+                backward();
+            }
+            if(result.value == KEY5){       // 5
+               stop();
+            }
+        }
+        
+    }
+
+
+
+    t=clock();
+    pc.printf("Hello PCF8574\n");
+    while (1) {
+
+        ultra_result = ultra.distance();
+        if(ultra_result > 0 && ultra_result < 14) {
+             if(in_line[0]==TRU && in_line[1]==TRU ){
+                stop(); 
+                mode = 2;
+                }
+
+            }
+
 
-        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;
+        if(mode == 1){
+            pc.printf("enter\r\n");
+            position = (float)readLine();
+            pc.printf("position: %f\r\n", position);
+            float proportional =(float)(position-2000.0); //e(t)
+            float derivative = (float)(proportional - last_proportional);
+            integral += proportional;
+            last_proportional = proportional;//
+            //pid 15/60000/2 - 85%
+            float power_difference = proportional/16.0+ (float)integral/
+            60000.0+ derivative*2.0;  //integral 변수 커지고 작아지는 값 확인하
+            pc.printf("proportional : %f integral %f derivative : %f \r\n", proportional, integral, derivative);
+            const int maximum = 255;
+            const int base = 155;
+            pc.printf("power_difference is %f\r\n" , power_difference);
+            if(power_difference > base ) power_difference = base;
+            if(power_difference < -base) power_difference = -base;
+            
+            
+        
+            if(power_difference < 0){ //오른쪽으로 빠
+    //            rightmotor.setSpeed((float)(base + power_difference) / (float)maximum);
+    //            leftmotor.setSpeed((float)base-power_difference/(float)maximum);    
+                pc.printf("right    : left %f, right %f \r\n",(float)(base+power_difference) / (float)maximum , (float)(base) / (float)maximum );
+                rightmotor.setSpeed((float)(base) / (float)maximum);
+                leftmotor.setSpeed((float)(base+power_difference) / (float)maximum );
+            } else{// 왼//쪽으로 빠짐.
+    //            rightmotor.setSpeed((float)(base + power_difference) / (float)maximum);
+    //            leftmotor.setSpeed((float)base-power_difference/(float)maximum); 
+                pc.printf("left    : left %f, right %f \r\n",(float)(base) / (float)maximum , (float)(base -power_difference) / (float)maximum );
+                    leftmotor.setSpeed((float)(base) / (float)maximum);
+                    rightmotor.setSpeed((float)(base -power_difference) / (float)maximum);
+                    
+                }
+                
+                if(in_line[0]&&in_line[1]&&in_line[2]&&in_line[3]&&in_line[4]){
+                    leftmotor.setSpeed(0);
+                    rightmotor.setSpeed(0);
+                    pc.printf("stop\r\n");
+                    //시간 모니터에 출력.
+                    break;
+                }         
+            }
+
+            if(mode == 2){
+                ultra_result = ultra.distance(); 
+                                  
+                for(int i=0; i<50; i++){
+                    data = readLine();
+                    if(in_line[2]==TRU && in_line[1]==FALS && in_line[3]==FALS && in_line[0]==FALS && in_line[4]==FALS) 
+                    {
+                        mode=1;
+                        break;
+                    }
+                    ultraLeft();
+                }
+            }
     }
-}
+                display.begin(true);
+                display.setTextSize(1);
+                display.setTextColor(WHITE);
+                display.setCursor(0,0);
+                t=clock()-t;
+                sprintf(time, "%f ms", (float)t*10); 
+                strcat(displaySentence, time);
+                display.println(displaySentence);
+                display.display();
+}
\ No newline at end of file