Alphabot
Dependencies: WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library
Fork of 15_FJ_1 by
Diff: main.cpp
- Revision:
- 97:8f3abd5cf5ce
- Parent:
- 88:bea4f2daa48c
- Child:
- 98:d8ead5e04047
--- a/main.cpp Fri May 31 13:00:04 2019 +0100 +++ b/main.cpp Wed Jun 12 08:20:53 2019 +0000 @@ -1,32 +1,377 @@ -/* mbed Microcontroller Library - * Copyright (c) 2018 ARM Limited - * SPDX-License-Identifier: Apache-2.0 - */ +#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) -#include "mbed.h" -#include "stats_report.h" - -DigitalOut led1(LED1); - -#define SLEEP_TIME 500 // (msec) -#define PRINT_AFTER_N_LOOPS 20 +// 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; -// 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 */); +// 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; - int count = 0; - while (true) { - // Blink LED and wait 0.5 seconds - led1 = !led1; - wait_ms(SLEEP_TIME); +// Reflective sensor +bool rightObs= false; +bool leftObs = false; + +// motor Driver +float fPwmAPeriod; +float fPwmAPulsewidth; +float fPwmBPeriod; +float fPwmBPulsewidth; - 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; +// 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; + } + } +} \ No newline at end of file