course1
Dependencies: WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library
main.cpp
- Committer:
- kmsmile2
- Date:
- 2019-06-12
- Revision:
- 97:8f3abd5cf5ce
- Parent:
- 88:bea4f2daa48c
- Child:
- 98:d8ead5e04047
File content as of revision 97:8f3abd5cf5ce:
#include "mbed.h" #include "RemoteIR.h" #include "ReceiverIR.h" #include "TB6612FNG.h" #include "TRSensors.h" #include "ultrasonic.h" #include "Adafruit_SSD1306.h" #define button_SENSORS 5 #define ADT7420_TEMP_REG (0x00) #define ADT7420_CONF_REG (0x03) #define EVAL_ADT7420_ADDR (0x48) #define PCF8574_ADDR (0x20) // create object DigitalOut dc(D8,1); DigitalOut rst(D9,1); I2C i2c(I2C_SDA, I2C_SCL); TB6612FNG motorDriver(D6, A1, A0, D5, A2, A3); Ultrasonic ultra(D3, D2, .1, false); ReceiverIR IR(D4); TRSensors trs; RawSerial pc(USBTX, USBRX, 115200); Adafruit_SSD1306_I2c gOled2(i2c,D9,0x7A,64,128); Ticker IRticker; Thread thread1; Thread thread2; Timer timer; // variables unsigned int sensorValues[button_SENSORS]; unsigned int last_proportional = 0; 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; 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; // Reflective sensor bool rightObs= false; bool leftObs = false; // motor Driver float fPwmAPeriod; float fPwmAPulsewidth; float fPwmBPeriod; 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); } } */ // opearte reflective sensors /* void reflective_thread() { while(1){ char data_write[2]; char data_read[2]; i2c.frequency(100000); i2c.read((PCF8574_ADDR<<1|0x01), data_read, 2, 0); pc.printf("hexa: %x, %x\r\n", data_read[0], data_read[1]); if(data_read[0]>>6 == 0b10) { pc.printf("Obstacle on left!\r\n"); data_write[0] = 0xDF; leftObs = true; } else if(data_read[0]>>6 == 0b01) { pc.printf("Obstacle on right!\r\n"); data_write[0] = 0xDF; rightObs = true; } else if(data_read[0]>>6 == 0b00) { pc.printf("Obstacle on front!\r\n"); data_write[0] = 0xDF; leftObs = true; rightObs = true; wait(3); } else { data_write[0]=0xFF; } i2c.write((PCF8574_ADDR<<1),data_write,1,0); } }*/ inline void update_display(){ gOled2.clearDisplay(); gOled2.setTextCursor(0,0); gOled2.printf("E-RON alphabot\r\n"); gOled2.printf("Timer: %.2f\r\n",t); gOled2.printf("V: %.0f\r\n",vel); gOled2.printf("P: %.2f\r\n",pval); gOled2.printf("I: %.5f\r\n",ival); gOled2.printf("D: %.2f\r\n",dval); gOled2.display(); } int main() { // thread1.start(ultrasonic_thread); // thread2.start(reflective_thread); RemoteIR::Format format; uint8_t buf[32]; int bitcount; update_display(); while(1) { // read the value of the code if (IR.getState() == ReceiverIR::Received) { bitcount = IR.getData(&format, buf, sizeof(buf) * 8); pc.printf("%d\r\n", bitcount); pc.printf("buf[2]: %d\r\n", buf[2]); button = buf[2]; } switch(button) { case 0x45: // ch- button (P value up) button = 0x1C; pval += 0.01; pval = fabs(pval); update_display(); // pc.printf("pval:%f\r\n", pval); wait(0.3); break; case 0x46: // ch common button (I value up) button = 0x1C; ival += 0.00001; ival = fabs(ival); update_display() ; // pc.printf("ival:%f\r\n", ival); wait(0.3); break; case 0x47: // ch+ button (D value up) button = 0x1C; dval += 0.05; dval = fabs(dval); update_display(); wait(0.3); break; case 0x44: // prev button (P value down) button = 0x1C; pval -= 0.01; pval = fabs(pval); update_display(); wait(0.3); break; case 0x40: // next button (I value down) button = 0x1C; ival -= 0.00001; ival = fabs(ival); update_display(); wait(0.3); button = 0x1C; break; case 0x43: // play/pause button (D value down) button = 0x1C; dval -= 0.05; dval = fabs(dval); update_display(); wait(0.3); button = 0x1C; break; case 0x07: // vol- button (velocity down) button = 0x1C; vel -= 5; update_display(); wait(0.3); button = 0x1C; break; case 0x15: // vol+ button (velocity up) button = 0x1C; vel += 5; update_display(); wait(0.3); button = 0x1C; break; case 0x16: // 0 button (Reset the error variables) int_err = 0; err = 0; prev_err = 0; wait(1); button = 0x1C; break; case 0x19: // 100+ button (Calibrate) pc.printf("calibrate!\r\n"); trs.calibrate(); pc.printf("calibrate done\r\n"); wait(0.2); button = 0x1C; break; case 0x18: // 2 button (move forward) motorDriver.user_forward(0.3,0.3); wait(0.1); button = 0x1C; break; case 0x52: // 8 button (move backward) motorDriver.user_backward(0.3,0.3); wait(0.1); button = 0x1C; break; case 0x5A: // 6 button (clockwise turn) motorDriver.user_right(0.3,0.1); wait(0.1); button = 0x1C; break; case 0x08: // 4 button (counter clockwise turn) motorDriver.user_left(0.1,0.3); wait(0.1); button = 0x1C; break; case 0x1C: // 5 button (motor stop) motorDriver.stop(); break; case 0x0D: // 200+ button (line tracer) t=0; timer.reset(); timer.start(); 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); 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; const float kp = pval; const float kd = dval; const float ki = ival; float power_difference = kp*err + kd*(err-prev_err) + ki*int_err; //+derivative; //error를 누적하는 것 prev_err = err; const float maximum=vel; if(power_difference > maximum) power_difference = maximum; if(power_difference < -maximum) power_difference = -maximum; if(power_difference<0) motorDriver.user_forward((maximum+15)/255,(maximum+power_difference)/255); else motorDriver.user_forward((maximum-power_difference)/255,(maximum-15)/255); pc.printf("position value: %d\r\n", position); //pc.printf("cnt: %d\r\n", cnt); for(int i=0;i<5;i++){ pc.printf("%d\r\n",sensorValues[i]); } if((sensorValues[0] > 650) && (sensorValues[1]>650) && (sensorValues[2]>650) && (sensorValues[3]>650) && (sensorValues[4]>650)) { // 5 IR sensor are on black timer.stop(); t=timer.read(); update_display(); button = 0x1C; break; } } break; 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); } case 0x4A: // 9 button (read position) int j = 0; while(j!=100) { j++; position=trs.readLine(sensorValues,0); pc.printf("%d\r\n",position); } button = 0x1C; break; dafault: // wrong button pc.printf("wrong button!\r\n"); break; } } }