![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
dd
Dependencies: Final HCSR04 TB6612FNG
main.cpp
- Committer:
- eunmango
- Date:
- 2019-06-16
- Revision:
- 97:b483e656bd14
- Parent:
- 88:bea4f2daa48c
File content as of revision 97:b483e656bd14:
#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); } //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; 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]; } } 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; } } 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() { 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(); } 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(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(); }