Alphabot_Group2
Alphabot
Dependencies: WS2812 PixelArray Adafruit_GFX HC_SR04_Ultrasonic_Library
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