final_8조
ss
Dependencies: WS2812 PixelArray Adafruit_GFX
Revision 0:27e31cadeb36, committed 2019-06-15
- Comitter:
- eunsong
- Date:
- Sat Jun 15 13:09:31 2019 +0000
- Child:
- 1:0896edc48011
- Commit message:
- yes
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Adafruit_GFX.lib Sat Jun 15 13:09:31 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/nkhorman/code/Adafruit_GFX/#7fb1d4d3525d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IRreflection.cpp Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,412 @@
+#include "IRreflection.h"
+
+
+
+// Base class data member initialization (called by derived class init())
+
+TRSensors::TRSensors(PinName MOSI,PinName MISO, PinName CLK, PinName CS) : _spi(MOSI,MISO,CLK), _spi_cs(CS)
+
+{
+
+ _spi.format(16,0);
+
+ _spi.frequency(2000000);
+
+ _spi_cs = 1;
+
+ _numSensors = NUMSENSORS;
+
+}
+
+
+
+
+
+
+
+void TRSensors::AnalogRead(int *sensor_values)
+
+{
+
+ int i,j,ch;
+
+ int values[] = {0,0,0,0,0,0};
+
+
+
+ for(j = 0;j < _numSensors + 1;j++)
+
+ {
+
+ ch =j;
+
+ _spi_cs = 0;
+
+ wait_us(2);
+
+ values[j] = _spi.write(ch<<12);
+
+ _spi_cs = 1;
+
+ wait_us(21);
+
+ values[j] = (values[j]>>6);
+
+ }
+
+
+
+ for(i = 0;i < _numSensors;i++)
+
+ {
+
+ sensor_values[i] = values[i+1];
+
+ }
+
+
+
+}
+
+
+
+// Reads the sensors 10 times and uses the results for
+
+// calibration. The sensor values are not returned; instead, the
+
+// maximum and minimum values found over time are stored internally
+
+// and used for the readCalibrated() method.
+
+void TRSensors::calibrate_init(int *calibratedMin, int *calibratedMax)
+
+{
+
+ for(int i=0;i<_numSensors;i++)
+
+ {
+
+ calibratedMin[i] = 1023;
+
+ calibratedMax[i] = 0;
+
+ }
+
+}
+
+void TRSensors::calibrate(int *sensor_values, int *calibratedMin, int *calibratedMax)
+
+{
+
+ int i=0;
+
+ int j=0;
+
+
+
+ for(j=0;j<10;j++)
+
+ {
+
+ AnalogRead(sensor_values);
+
+ for(i=0;i<_numSensors;i++)
+
+ {
+
+ // set the max we found THIS time
+
+ if(j == 0 || max_sensor_values[i] < sensor_values[i])
+
+ max_sensor_values[i] = sensor_values[i];
+
+
+
+ // set the min we found THIS time
+
+ if(j == 0 || min_sensor_values[i] > sensor_values[i])
+
+ min_sensor_values[i] = sensor_values[i];
+
+ }
+
+ }
+
+
+
+ // record the min and max calibration values
+
+ 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];
+
+ }
+
+}
+
+
+
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+
+// 0 corresponds to the minimum value read by calibrate() and 1000
+
+// corresponds to the maximum value. Calibration values are
+
+// stored separately for each sensor, so that differences in the
+
+// sensors are accounted for automatically.
+
+void TRSensors::readCalibrated(int *sensor_values, int *calibratedMin, int *calibratedMax)
+
+{
+
+ int i;
+
+
+
+ // read the needed values
+
+ AnalogRead(sensor_values);
+
+
+
+ for(i=0;i<_numSensors;i++)
+
+ {
+
+ int denominator;
+
+
+
+ denominator = calibratedMax[i] - calibratedMin[i];
+
+
+
+ int x = 0;
+
+
+
+
+
+ if(denominator != 0){
+
+ if(((signed int)sensor_values[i] - (signed int)calibratedMin[i])<0){
+
+ x = 0;}
+
+ else {
+
+ x = ((sensor_values[i] - calibratedMin[i])*1000/denominator);
+
+ }
+
+ }
+
+
+
+ if( x > 1000) x = 1000;
+
+
+
+ sensor_values[i] = x;
+
+
+
+
+
+}
+
+
+
+}
+
+
+
+
+
+
+
+// Operates the same as read calibrated, but also returns an
+
+// estimated position of the robot with respect to a line. The
+
+// estimate is made using a weighted average of the sensor indices
+
+// multiplied by 1000, so that a return value of 0 indicates that
+
+// the line is directly below sensor 0, a return value of 1000
+
+// indicates that the line is directly below sensor 1, 2000
+
+// indicates that it's below sensor 2000, etc. Intermediate
+
+// values indicate that the line is between two sensors. The
+
+// formula is:
+
+//
+
+// 0*value0 + 1000*value1 + 2000*value2 + ...
+
+// --------------------------------------------
+
+// value0 + value1 + value2 + ...
+
+//
+
+// By default, this function assumes a dark line (high values)
+
+// surrounded by white (low values). If your line is light on
+
+// black, set the optional second argument white_line to true. In
+
+// this case, each sensor value will be replaced by (1000-value)
+
+// before the averaging.
+
+
+
+int TRSensors::readLine(int *sensor_values , int *calibratedMin, int *calibratedMax, int *online, char white_line)
+
+{
+
+ char i, on_line = 0;
+
+ long int avg; // this is for the weighted total, which is long
+
+ // before division
+
+ int sum; // this is for the denominator which is <= 64000
+
+ static int last_value=0; // assume initially that the line is left.
+
+
+
+ readCalibrated(sensor_values, calibratedMin, calibratedMax);
+
+
+
+ online[0] = 0;
+
+ avg = 0;
+
+ sum = 0;
+
+
+
+ for(i=0;i<_numSensors;i++) {
+
+
+
+ long int value = sensor_values[i];
+
+
+
+ //if(!white_line) value = 1000-value;
+
+ if(white_line){
+
+ value = 1000-value;
+
+ //sensor_values[i] = (1000 - sensor_values[i])/100;
+
+ }
+
+
+
+ //sensor_values[i] = value;
+
+ // keep track of whether we see the line at all
+
+ if(value > 300) {
+
+ online[0] = 1;
+
+ }
+
+
+
+ // only average in values that are above a noise threshold
+
+
+ if(value > 30) {
+
+
+
+ avg += value*(1000*i);//(value/100)*(1000*i) // * sqrt(100,i);
+
+ sum += value; //(value/100);
+
+ }
+
+ }
+
+ /*
+
+ if(!on_line)
+
+ {
+
+ // If it last read to the left of center, return 0.
+
+ if(last_value < (_numSensors-1)*1000/2)
+
+ return 0;
+
+
+
+ // If it last read to the right of center, return the max.
+
+ else
+
+ return (_numSensors-1)*1000;
+
+ }
+
+ */
+
+
+
+ last_value = avg/sum;
+
+
+
+ // if(sum == 0) last_value = 0;
+
+
+
+ return last_value;
+
+}
+
+
+
+int TRSensors::sqrt(int m, int k){
+
+ int temp = m;
+
+ if(k == 0) m = 1;
+
+ else{
+
+ for(int i=0; i<(k-1); i++){
+
+ m = temp *m;
+
+ }
+
+ }
+
+ return m;
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/IRreflection.h Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,35 @@
+#ifndef TRSensors_h
+#define TRSensors_h
+
+#include "mbed.h"
+#define NUMSENSORS 5
+
+class TRSensors
+{
+ public:
+
+ TRSensors(PinName MOSI,PinName MISO, PinName CLK, PinName CS);
+
+ void calibrate_init(int *calibratedMin, int *calibratedMax);
+
+ void calibrate(int *sensor_values, int *calibratedMin, int *calibratedMax);
+
+ void AnalogRead(int *sensor_values);
+
+ void readCalibrated(int *sensor_values, int *calibratedMin, int *calibratedMax);
+
+ int readLine(int *sensor_values , int *calibratedMin, int *calibratedMax, int *online, char white_line = 0);
+
+ int sqrt(int m, int k);
+
+ char _numSensors;
+ int max_sensor_values[NUMSENSORS];
+ int min_sensor_values[NUMSENSORS];
+
+ private:
+ SPI _spi;
+ DigitalOut _spi_cs;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PCF8574.cpp Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,20 @@
+#include "PCF8574.h"
+#include "mbed.h"
+
+PCF8574::PCF8574(PinName sda, PinName scl, int address)
+ : _i2c(sda, scl) {
+ _address = address;
+ _i2c.frequency(100000);
+}
+
+int PCF8574::read() {
+ char foo[1];
+ _i2c.read(_address, foo, 1);
+ return foo[0];
+}
+
+void PCF8574::write(int data) {
+ char foo[1];
+ foo[0] = data;
+ _i2c.write(_address, foo, 1);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PCF8574.h Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+
+#ifndef MBED_PCF8574_H
+#define MBED_PCF8574_H
+
+/** Interface to the popular PCF8574 I2C 8 Bit IO expander */
+class PCF8574 {
+public:
+ /** Create an instance of the PCF8574 connected to specfied I2C pins, with the specified address.
+ *
+ * @param sda The I2C data pin
+ * @param scl The I2C clock pin
+ * @param address The I2C address for this PCF8574
+ */
+ PCF8574(PinName sda, PinName scl, int address);
+
+ /** Read the IO pin level
+ *
+ * @return The byte read
+ */
+ int read();
+
+ /** Write to the IO pins
+ *
+ * @param data The 8 bits to write to the IO port
+ */
+ void write(int data);
+
+private:
+ I2C _i2c;
+ int _address;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PixelArray.lib Sat Jun 15 13:09:31 2019 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/chris/code/PixelArray/#b45a70faaa83
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RemoteIR.lib Sat Jun 15 13:09:31 2019 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/shintamainjp/code/RemoteIR/#70f3af9237d9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/WS2812.lib Sat Jun 15 13:09:31 2019 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/bridadan/code/WS2812/#6e647820f587
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/hcsr04.cpp Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,63 @@
+#include "hcsr04.h"
+#include "mbed.h"
+/*
+*HCSR04.cpp
+*/
+HCSR04::HCSR04(PinName t, PinName e, RawSerial pc,int flag, int limit) : trig(t), echo(e), _pc(pc), flag(flag), _limit(limit) {}
+
+void HCSR04::Trig()
+{
+ timer.reset(); //reset timer
+ trig=0; // trigger low
+ wait_us(2); // wait
+ trig=1; // trigger high
+ wait_us(10);
+ trig=0; // trigger low
+}
+
+/*
+void HCSR04::setMode(bool mode)
+{
+ _repeat = mode;
+}
+
+void HCSR04::clearStatus()
+{
+ _done = 0;
+}
+
+int HCSR04::getStatus()
+{
+ return _done;
+}*/
+
+
+
+ long HCSR04::echo_duration() {
+
+ Trig();
+ while(!echo); // start pulseIN
+ timer.start();
+ while(echo);
+ timer.stop();
+ return timer.read_us();
+
+}
+
+//return distance in cm
+void HCSR04::distance(){
+ // _done++;
+ duration = echo_duration();
+ distance_cm = int((duration/2)/29.1) ;
+ _pc.printf("distance: %d\r\n", distance_cm);
+ if(distance_cm <= _limit){
+ flag = 1;
+ _pc.printf("\t\tFlag on distance: %d\r\n", distance_cm);
+
+ }
+}
+
+long HCSR04::returndistance()
+{
+ return distance_cm;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/hcsr04.h Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,33 @@
+#ifndef hcsr04_H
+#define hcsr04_H
+#include "mbed.h"
+
+
+
+class HCSR04 {
+ public:
+ HCSR04(PinName t, PinName e, RawSerial pc, int flag, int limit);
+ long echo_duration();
+ void distance();
+ void Trig();
+ long returndistance();
+
+ /*void setMode(bool mode);
+ int getStatus();
+ void clearStatus();*/
+ int flag;
+
+ private:
+ /*bool _repeat;
+ float _interval;
+ int _done;*/
+ DigitalOut trig;
+ DigitalIn echo;
+ RawSerial _pc;
+ Timer timer;
+ long duration;
+ int distance_cm;
+ int _limit;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Jun 15 13:09:31 2019 +0000
@@ -0,0 +1,606 @@
+#include "mbed.h"
+#include "IRreflection.h"
+#include "ReceiverIR.h"
+#include "PCF8574.h"
+#include "hcsr04.h"
+#include "Adafruit_SSD1306.h"
+#include "WS2812.h"
+#include "PixelArray.h"
+
+#define WS2812_BUF 77 //number of LEDs in the array
+#define NUM_COLORS 6 //number of colors to store in the array
+#define NUM_STEPS 8 //number of steps between colors
+
+Timer total;
+Timer Etime;
+Timer Otime;
+Timer AngleTimer;
+
+//WS2812 neopixel================
+PixelArray px(WS2812_BUF);
+WS2812 ws(D7, WS2812_BUF, 6,17,9,14); //nucleo-f411re
+Thread neothread;
+
+// temperary=========================
+RawSerial pc(USBTX, USBRX, 115200);
+//ultrasonic sensor=====================
+int limit = 5;
+HCSR04 sensor(D3, D2,pc,0,limit);
+int turn = 0;
+int Ultra_distance = 0;
+
+//PCF8574============================
+PCF8574 io(I2C_SDA,I2C_SCL,0x40);
+//IR_Reflection : declaration========
+TRSensors TR(D11,D12,D13,D10);
+
+static int sensor_for_end[NUMSENSORS];
+static int sensor_val[NUMSENSORS];
+static int calMin[NUMSENSORS];
+static int calMax[NUMSENSORS];
+//==================================
+//==============Motor===============
+PwmOut PWMA(D6); //PB_10,D6
+PwmOut PWMB(D5);
+
+DigitalOut AIN1(A1);
+DigitalOut AIN2(A0);
+DigitalOut BIN1(A2);
+DigitalOut BIN2(A3);
+
+#define MIDDLE 2000
+int R_PWM;
+int L_PWM;
+int weight; int P_weight;
+int P_temp; int Fix_temp;
+int print_c;
+int den;
+static int on_line[1];
+typedef enum{LEFT = 0, RIGHT} DIRE;
+DIRE direction;
+DIRE Over_direction;
+DIRE P_direction;
+DIRE Obs_direction;
+//==================================
+//IR_Remote : declaration===========
+ReceiverIR ir_rx(D4);
+
+RemoteIR::Format format;
+uint8_t IR_buf[32];
+uint32_t IR_buf_sum;
+int bitcount;
+//==========flag===================
+
+typedef enum{NOK = 0, YOK}flag;
+// Nessary change to enum!!!
+flag flag_cal_Min;
+flag flag_cal_Max;
+flag flag_start;
+flag flag_over;
+flag flag_out;
+flag flag_IR;
+flag flag_end;
+flag flag_angle;
+flag flag_obstacle;
+flag flag_distance;
+flag flag_neo;
+//==============================
+void Initialization(void);
+void Motor_init(void);
+void Motor_Stop(void);
+void Calibration(void);
+void Driving(void);
+void Actuator(void);
+void Distance_check(void);
+void Obstacle_check(void);
+void Actuator_Obstacle(int dir);
+int End_check(int *sensor_values);
+int color_set(uint8_t red,uint8_t green, uint8_t blue);
+int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber);
+void NeopixelOn(void);
+
+//===========================OLED
+class I2CPreInit : public I2C
+{
+public:
+ I2CPreInit(PinName sda, PinName scl) : I2C(sda, scl)
+ {
+ frequency(400000);
+ start();
+ };
+};
+
+I2C i2c(D14, D15);
+Adafruit_SSD1306_I2c oled(i2c, D9, 0x78, 64, 128);
+
+int main(){
+ neothread.start(&NeopixelOn);
+ oled.clearDisplay();
+ oled.printf("Welcome to Alphabot\r\n\r\n");
+ oled.display();
+
+ Initialization();
+
+ Driving();
+
+ if(flag_end){
+ oled.clearDisplay();
+ int duration = total.read();
+ oled.printf("Congratulation!! \r\n");
+ oled.printf("Time is %.3f", total.read());
+ oled.display();
+ flag_neo = YOK;
+ }
+
+
+}
+//====================================================
+//==================initialization====================
+//====================================================
+void Initialization(){
+
+ //추후 다른 변수 초기화 전부 이곳에!!!
+
+ flag_over = NOK;
+ flag_IR = NOK;
+ flag_distance = NOK;
+
+ TR.calibrate_init(calMin,calMax);
+
+ Motor_init();
+
+ Calibration();
+
+}
+
+void Motor_Stop(){
+ pc.printf("===============================Motor Stop!\r\n");
+ AIN1 = 0;
+ AIN2 = 0;
+ BIN1 = 0;
+ BIN2 = 0;
+ PWMB.pulsewidth_us(0);
+ PWMA.pulsewidth_us(0);
+}
+
+void Motor_init(){
+ AIN1 = 0;
+ AIN2 = 1;
+ BIN1 = 0;
+ BIN2 = 1;
+ PWMB.period_us(500);
+ PWMA.period_us(500);
+
+ den = 0;
+ R_PWM = 0;
+ L_PWM = 0;
+ weight= 0;
+ print_c = 0;
+
+
+}
+void Calibration(){
+
+ while(flag_cal_Max == NOK){
+
+ if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
+ for(int i =0 ; i<4; i ++){
+ IR_buf_sum |=(IR_buf[i]<<8*(3-i));
+ }
+ pc.printf("%X\r\n",IR_buf_sum);
+ if(IR_buf_sum == 0x00FF0CF3){
+ io.write(0x7F);
+ TR.calibrate(sensor_val, calMin, calMax);
+ wait(0.5);
+ io.write(0xFF);
+ for(int i = 0; i<5; i++){
+ pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
+ pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
+ }
+ pc.printf("================================\r\n");
+
+ flag_cal_Max = YOK;
+ }
+ IR_buf_sum = 0;
+ }
+ }
+
+ while(flag_cal_Min == NOK){
+
+ if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
+ for(int i =0 ; i<4; i ++){
+ IR_buf_sum |=(IR_buf[i]<<8*(3-i));
+ }
+ pc.printf("%X\r\n",IR_buf_sum);
+ if(IR_buf_sum == 0x00FF18E7){
+ io.write(0xBF);
+ TR.calibrate(sensor_val, calMin, calMax);
+ wait(0.5);
+ io.write(0xFF);
+for(int i = 0; i<5; i++){
+pc.printf("Min_sensor_value[%d] = %d\r\n",i+1,calMin[i]);
+pc.printf("Max_sensor_value[%d] = %d\r\n",i+1,calMax[i]);
+}
+pc.printf("================================\r\n");
+ flag_cal_Min = YOK;
+ }
+ IR_buf_sum = 0;
+ }
+ }
+
+
+
+//=================start===============================
+ while(flag_start == NOK){
+
+ if ((ir_rx.getState() == ReceiverIR::Received)&&(ir_rx.getData(&format, IR_buf, sizeof(IR_buf) * 8)!=0)) {
+ for(int i =0 ; i<4; i ++){
+ IR_buf_sum |=(IR_buf[i]<<8*(3-i));
+ }
+
+ pc.printf("%X\r\n",IR_buf_sum);
+ if(IR_buf_sum == 0x00FF5EA1){
+ pc.printf("===============start!===============\r\n");
+ flag_start = YOK;
+ }
+ IR_buf_sum = 0;
+ }
+ }
+//================================================================
+}
+
+
+
+
+
+
+
+
+//Using logic control=================================================================
+void Driving(){
+
+
+ total.start();
+
+ Etime.start();
+ while(!flag_end){
+ Etime.reset();
+
+
+ // Obstacle_check();
+
+ if(flag_distance == NOK){
+ Actuator();
+ }
+
+
+ Distance_check();
+
+
+ do{
+ }while(Etime.read()<0.00300001);
+ }
+
+}
+
+
+int End_check(int *sensor_values){
+ int avg = 0;
+ int sum = 0;
+
+ for(int i = 0; i < NUMSENSORS; i++){
+ sum += sensor_values[i];
+ }
+ avg = sum / NUMSENSORS;
+ //pc.printf("\tavg function: %d\r\n",avg);
+ return avg;
+}
+
+
+
+void Actuator(){
+
+ den = 30;
+
+ //int temp = TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE;
+ int temp = TR.readLine(sensor_val, calMin, calMax,on_line,1);
+ //pc.printf("new temp: %d\r\n",temp);
+
+ TR.readCalibrated(sensor_for_end, calMin, calMax);
+ int avg = End_check(sensor_for_end);
+
+ if(avg <= 100){
+ pc.printf("avg:%d\r\n",avg);
+ Motor_Stop();
+ flag_end = YOK;
+ total.stop();
+ }
+
+ temp = temp - MIDDLE;
+
+ if(temp>=0) direction = LEFT;
+ else direction = RIGHT;
+
+
+ weight = abs(temp)/den;
+
+ if((print_c%500) == 0){
+ pc.printf("flag_out = %d\r\n", flag_out);
+ pc.printf("temp = %d\r\n", temp);
+ pc.printf("online = %d\r\n", on_line[0]);
+ }
+
+ R_PWM = 140;
+ L_PWM = 140;
+
+
+
+ //if(weight > (2000/den)*2/3) weight = (2000/den)*3/4;
+
+ if(weight >= (2000/den)*3/4){
+ if(direction == LEFT){
+ PWMA.pulsewidth_us(L_PWM+0.0*weight);
+ PWMB.pulsewidth_us(R_PWM-2.1*weight);
+ }
+ else{
+ PWMA.pulsewidth_us(L_PWM-2.1*weight);
+ PWMB.pulsewidth_us(R_PWM+0.0*weight);
+ }
+ }else{
+
+ if(direction == LEFT){
+ PWMA.pulsewidth_us(L_PWM+0.0*weight);
+ PWMB.pulsewidth_us(R_PWM-1.4*weight);
+ }
+ else{
+ PWMA.pulsewidth_us(L_PWM-1.4*weight);
+ PWMB.pulsewidth_us(R_PWM+0.0*weight);
+ }
+ }
+
+
+ if(weight >= (2000/den)*2/3 && flag_over == NOK)
+ {
+ flag_over = YOK;
+ P_direction = direction;
+ P_weight = weight;
+ }
+
+ if((flag_over == YOK) && (abs(weight)<= (2000/den)*1/5)&&on_line[0]==1)
+ {
+ if(P_weight >=(2000/den)*2/3 && P_weight<=(2000/den)*4/5){
+ if(P_direction == LEFT){
+ PWMA.pulsewidth_us(L_PWM-P_weight);
+ PWMB.pulsewidth_us(R_PWM);
+ }
+ else{
+ PWMA.pulsewidth_us(L_PWM);
+ PWMB.pulsewidth_us(R_PWM-P_weight);
+ }
+ }
+ if(P_weight>=(2000/den)*4/5){
+ if(P_direction == LEFT){
+ PWMA.pulsewidth_us(L_PWM-1.0*P_weight);
+ PWMB.pulsewidth_us(R_PWM+1.0*P_weight);
+ }
+ else{
+ PWMA.pulsewidth_us(L_PWM+1.0*P_weight);
+ PWMB.pulsewidth_us(R_PWM-1.0*P_weight);
+ }
+ }
+ // pc.printf("I'm Here\r\n");
+
+ wait(0.12);
+ flag_over = NOK;
+ }
+
+ P_temp = temp;
+ print_c++;
+}
+
+
+void Distance_check(){
+
+ if(flag_distance == NOK){
+ sensor.distance();
+ Ultra_distance = sensor.returndistance();
+ //pc.printf("distance = %d\r\n",flag_distance);
+ if((Ultra_distance >= 16 && Ultra_distance <= 19)) flag_distance = YOK;
+ }
+
+ if(flag_distance == YOK){
+ while(1){
+ // pc.printf("distance check. turn left!!\r\n");
+ PWMB.pulsewidth_us(100);
+ PWMA.pulsewidth_us(100);
+ AIN1.write(1);
+ AIN2.write(0);
+ BIN1.write(0);
+ BIN2.write(1);
+ //pc.printf("ABS == ")
+ if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)){
+ Motor_init();
+ flag_distance = NOK;
+ PWMB.pulsewidth_us(0);
+ PWMA.pulsewidth_us(0);
+ break;
+ }
+ }
+ }
+}
+
+void Obstacle_check(){
+
+ //io.write 체크
+ //flag_IR 변경 YOK
+ // pc.printf("flag_out = %x\r\n", io.read());
+ if(io.read() == 0x7F){ //왼쪽읽음
+ flag_IR = YOK;
+ flag_obstacle = YOK;
+ Obs_direction = LEFT;
+ }
+ else if(io.read() == 0xbF) //오른쪽 읽음
+ {
+ flag_IR = YOK;
+ flag_obstacle = YOK;
+ Obs_direction = RIGHT;
+ }
+
+ if(flag_IR){
+ Otime.start();
+ //flag_IR = NOK;
+ // pc.printf("\tActuator Obstacel start\r\n");
+
+ while(flag_obstacle == YOK){
+ //while(io.read() != 0xFF){
+
+ Actuator_Obstacle(Obs_direction);
+ pc.printf("obstacle!\r\n");
+ //pc.printf("\t\tActuator Obstacel start\r\n");
+
+ }
+ Otime.stop();
+ Otime.reset();
+ flag_IR = NOK;
+ }
+
+
+
+
+}
+
+void Actuator_Obstacle(int dir){
+
+ R_PWM = 100;
+ L_PWM = 100;
+
+if(Otime.read() <= 0.5){
+
+ if(dir == 0){
+ PWMA.pulsewidth_us(L_PWM-20);
+ PWMB.pulsewidth_us(R_PWM-100);
+ }
+ else if(dir == 1 ){
+ PWMA.pulsewidth_us(L_PWM-100);
+ PWMB.pulsewidth_us(R_PWM-40);
+ }
+
+}else if(Otime.read() <= 1.0){
+
+ if(dir == 0){
+ PWMA.pulsewidth_us(L_PWM+5);
+ PWMB.pulsewidth_us(R_PWM);
+ }
+ else if(dir == 1 ){
+ PWMA.pulsewidth_us(L_PWM+5);
+ PWMB.pulsewidth_us(R_PWM);
+ }
+
+}else if(Otime.read() <= 2.0){
+
+ if(dir == 0){
+ PWMA.pulsewidth_us(L_PWM-100);
+ PWMB.pulsewidth_us(R_PWM-40);
+ }
+ else if(dir == 1 ){
+ PWMA.pulsewidth_us(L_PWM-20);
+ PWMB.pulsewidth_us(R_PWM-100);
+ }
+
+}else{
+
+ if((abs(TR.readLine(sensor_val, calMin, calMax,on_line,1) - MIDDLE)<=400 && on_line[0] == 1)) flag_obstacle = NOK;
+
+ PWMA.pulsewidth_us(L_PWM);
+ PWMB.pulsewidth_us(R_PWM);
+
+ }
+
+}
+
+
+int color_set(uint8_t red,uint8_t green, uint8_t blue)
+{
+ return ((red<<16) + (green<<8) + blue);
+}
+
+// 0 <= stepNumber <= lastStepNumber
+int interpolate(int startValue, int endValue, int stepNumber, int lastStepNumber)
+{
+ return (endValue - startValue) * stepNumber / lastStepNumber + startValue;
+}
+
+void NeopixelOn(){
+ int colorIdx = 0;
+ int colorTo = 0;
+ int colorFrom = 0;
+
+ uint8_t ir = 0;
+ uint8_t ig = 0;
+ uint8_t ib = 0;
+
+ ws.useII(WS2812::PER_PIXEL); // use per-pixel intensity scaling
+
+ // set up the colours we want to draw with
+ int colorbuf[NUM_COLORS] = {0x2f0000,0x2f2f00,0x002f00,0x002f2f,0x00002f,0x2f002f};
+
+ // Now the buffer is written, write it to the led array.
+ while (flag_neo)
+ {
+ //get starting RGB components for interpolation
+ std::size_t c1 = colorbuf[colorFrom];
+ std::size_t r1 = (c1 & 0xff0000) >> 16;
+ std::size_t g1 = (c1 & 0x00ff00) >> 8;
+ std::size_t b1 = (c1 & 0x0000ff);
+
+ //get ending RGB components for interpolation
+ std::size_t c2 = colorbuf[colorTo];
+ std::size_t r2 = (c2 & 0xff0000) >> 16;
+ std::size_t g2 = (c2 & 0x00ff00) >> 8;
+ std::size_t b2 = (c2 & 0x0000ff);
+
+ for (int i = 0; i <= NUM_STEPS; i++)
+ {
+ ir = interpolate(r1, r2, i, NUM_STEPS);
+ ig = interpolate(g1, g2, i, NUM_STEPS);
+ ib = interpolate(b1, b2, i, NUM_STEPS);
+
+ //write the color value for each pixel
+ px.SetAll(color_set(ir,ig,ib));
+
+ //write the II value for each pixel
+ px.SetAllI(32);
+
+ for (int i = WS2812_BUF; i >= 0; i--)
+ {
+ ws.write(px.getBuf());
+ }
+ }
+
+ colorFrom = colorIdx;
+ colorIdx++;
+
+ if (colorIdx >= NUM_COLORS)
+ {
+ colorIdx = 0;
+ }
+
+ colorTo = colorIdx;
+ }
+}
+
+
+/*
+else if(Otime.read() <= 2.0){
+
+ if(dir == 0){
+ PWMA.pulsewidth_us(L_PWM+5);
+ PWMB.pulsewidth_us(R_PWM);
+ }
+ else if(dir == 1 ){
+ PWMA.pulsewidth_us(L_PWM+5);
+ PWMB.pulsewidth_us(R_PWM);
+ }
+
+}
+*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-os.lib Sat Jun 15 13:09:31 2019 +0000 @@ -0,0 +1,1 @@ +https://github.com/ARMmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48