Embedded Class - Final Project1 / Mbed OS mbed-os-example-FinalReal_copy

Final

Dependencies:   IRremote HCSR04 TB6612FNG

main.cpp@97:59d348745d96, 2019-06-16 (annotated)

Committer:
eunmango
Date:
Sun Jun 16 04:51:51 2019 +0000
Revision:
97:59d348745d96
Parent:
88:bea4f2daa48c 
s

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eunmango 97:59d348745d96 1 #include "mbed.h"
eunmango 97:59d348745d96 2 #include <iostream>
eunmango 97:59d348745d96 3 #include "HCSR04.h"
eunmango 97:59d348745d96 4 #include <list>
eunmango 97:59d348745d96 5 #include <string.h>
eunmango 97:59d348745d96 6 #include <IRremote.h>
eunmango 97:59d348745d96 7 #include "TB6612FNG.h"
eunmango 97:59d348745d96 8 //#define PCF8574_ADDR (0x40)
eunmango 97:59d348745d96 9 #define LEFT 127
eunmango 97:59d348745d96 10 #define RIGHT 191
eunmango 97:59d348745d96 11 #define NUMSENSORS 5
eunmango 97:59d348745d96 12 #define MAX 1023
eunmango 97:59d348745d96 13 #define MIN 0
eunmango 97:59d348745d96 14 #define TRU 1
eunmango 97:59d348745d96 15 #define FALS 0
eunmango 97:59d348745d96 16 #define MaxSpeed 255// max speed of the robot
eunmango 97:59d348745d96 17 #define BaseSpeed 155
eunmango 97:59d348745d96 18 #define speedt 180
eunmango 97:59d348745d96 19
eunmango 97:59d348745d96 20 #include "SSD1306-Library.h"
eunmango 97:59d348745d96 21 #include "FreeSans9pt7b.h"
eunmango 97:59d348745d96 22 #include <time.h>
eunmango 97:59d348745d96 23 #include <math.h>
eunmango 97:59d348745d96 24
eunmango 97:59d348745d96 25 #define KEY2 0xFF18E7 //Key:2
eunmango 97:59d348745d96 26 #define KEY8 0xFF52AD //Key:8
eunmango 97:59d348745d96 27 #define KEY4 0xFF08F7 //Key:4
eunmango 97:59d348745d96 28 #define KEY6 0xFF5AA5 //Key:6
eunmango 97:59d348745d96 29 #define KEY1 0xFF0CF3 //Key:1
eunmango 97:59d348745d96 30 #define KEY3 0xFF5EA1 //Key:3
eunmango 97:59d348745d96 31 #define KEY5 0xFF1CE3 //Key:5
eunmango 97:59d348745d96 32
eunmango 97:59d348745d96 33
eunmango 97:59d348745d96 34 //I2C i2c(I2C_SDA, I2C_SCL); //D15,D14
eunmango 97:59d348745d96 35 SPI spi(D11, D12, D13);
eunmango 97:59d348745d96 36 DigitalOut cs(D10,1);
eunmango 97:59d348745d96 37 Serial pc(PA_2, PA_3, 115200); //D1 D0
eunmango 97:59d348745d96 38 IRrecv irrecv(D4);
eunmango 97:59d348745d96 39
eunmango 97:59d348745d96 40
eunmango 97:59d348745d96 41 int mode=1;
eunmango 97:59d348745d96 42 int index=0;
eunmango 97:59d348745d96 43 int rx_buffer[10];
eunmango 97:59d348745d96 44 char arr[16];
eunmango 97:59d348745d96 45 volatile int flag = 0;
eunmango 97:59d348745d96 46 float interval=0.1; //100ms
eunmango 97:59d348745d96 47 decode_results result;
eunmango 97:59d348745d96 48
eunmango 97:59d348745d96 49
eunmango 97:59d348745d96 50
eunmango 97:59d348745d96 51 /////////sensors////////////
eunmango 97:59d348745d96 52
eunmango 97:59d348745d96 53 int temp=0;
eunmango 97:59d348745d96 54 int calibratedMin[NUMSENSORS];
eunmango 97:59d348745d96 55 int calibratedMax[NUMSENSORS];
eunmango 97:59d348745d96 56 unsigned int sensor_values[NUMSENSORS];
eunmango 97:59d348745d96 57 int in_line[NUMSENSORS];
eunmango 97:59d348745d96 58 int ultra_flag=0;
eunmango 97:59d348745d96 59 float position;
eunmango 97:59d348745d96 60
eunmango 97:59d348745d96 61 /////////sensors////////////
eunmango 97:59d348745d96 62
eunmango 97:59d348745d96 63 /////////Ultrasonic////////////
eunmango 97:59d348745d96 64 Ultrasonic ultra(D3, D2);
eunmango 97:59d348745d96 65 int ultra_result=10;
eunmango 97:59d348745d96 66 /////////Ultrasonic////////////
eunmango 97:59d348745d96 67
eunmango 97:59d348745d96 68
eunmango 97:59d348745d96 69 //////////motor////////////
eunmango 97:59d348745d96 70 PwmOut motor_pwmA(PB_10); //D6
eunmango 97:59d348745d96 71 PwmOut motor_pwmb(PB_4); // D5
eunmango 97:59d348745d96 72 AnalogIn rightMotor2(A0);
eunmango 97:59d348745d96 73 AnalogIn rightMotor1(A1);
eunmango 97:59d348745d96 74 AnalogIn leftMotor2(A3);
eunmango 97:59d348745d96 75 AnalogIn leftMotor1(A2);
eunmango 97:59d348745d96 76
eunmango 97:59d348745d96 77 TB6612FNG leftmotor(PB_10, A1, A0);
eunmango 97:59d348745d96 78 TB6612FNG rightmotor(PB_4, A2, A3);
eunmango 97:59d348745d96 79 float last_proportional;
eunmango 97:59d348745d96 80 float integral;
eunmango 97:59d348745d96 81 float kp = 0.5;
eunmango 97:59d348745d96 82 float kd = 0.1;
eunmango 97:59d348745d96 83 int lastError;
eunmango 97:59d348745d96 84 //////////motor////////////
eunmango 97:59d348745d96 85
eunmango 97:59d348745d96 86 void IRForward(void)
eunmango 97:59d348745d96 87 {
eunmango 97:59d348745d96 88 leftmotor.setSpeed(0.1);
eunmango 97:59d348745d96 89 rightmotor.setSpeed(0.1);
eunmango 97:59d348745d96 90
eunmango 97:59d348745d96 91 }
eunmango 97:59d348745d96 92
eunmango 97:59d348745d96 93 void ultraLeft(){
eunmango 97:59d348745d96 94 leftmotor.setSpeed(-0.13);
eunmango 97:59d348745d96 95 rightmotor.setSpeed(0.15);
eunmango 97:59d348745d96 96 }
eunmango 97:59d348745d96 97
eunmango 97:59d348745d96 98 void left(void)
eunmango 97:59d348745d96 99 {
eunmango 97:59d348745d96 100 leftmotor.setSpeed(0.03);
eunmango 97:59d348745d96 101 rightmotor.setSpeed(0.15);
eunmango 97:59d348745d96 102 }
eunmango 97:59d348745d96 103
eunmango 97:59d348745d96 104
eunmango 97:59d348745d96 105 void right(void)
eunmango 97:59d348745d96 106 {
eunmango 97:59d348745d96 107 leftmotor.setSpeed(0.15);
eunmango 97:59d348745d96 108 rightmotor.setSpeed(0.03);
eunmango 97:59d348745d96 109 }
eunmango 97:59d348745d96 110
eunmango 97:59d348745d96 111 void stop(void)
eunmango 97:59d348745d96 112 {
eunmango 97:59d348745d96 113 leftmotor.setSpeed(0);
eunmango 97:59d348745d96 114 rightmotor.setSpeed(0);
eunmango 97:59d348745d96 115 }
eunmango 97:59d348745d96 116
eunmango 97:59d348745d96 117 void CalibrationMotor(void)
eunmango 97:59d348745d96 118 {
eunmango 97:59d348745d96 119 leftmotor.setSpeed(0.15);
eunmango 97:59d348745d96 120 rightmotor.setSpeed(-0.15);
eunmango 97:59d348745d96 121 }
eunmango 97:59d348745d96 122
eunmango 97:59d348745d96 123 void backward(void)
eunmango 97:59d348745d96 124 {
eunmango 97:59d348745d96 125 leftmotor.setSpeed(-0.1);
eunmango 97:59d348745d96 126 rightmotor.setSpeed(-0.1);
eunmango 97:59d348745d96 127 }
eunmango 97:59d348745d96 128
mbed_official 82:abf1b1785bd7 129
eunmango 97:59d348745d96 130 //bool pcf8574_write(uint8_t data){
eunmango 97:59d348745d96 131 // return i2c.write(PCF8574_ADDR, (char*) &data, 1, 0) == 0;
eunmango 97:59d348745d96 132 //}
eunmango 97:59d348745d96 133 //
eunmango 97:59d348745d96 134 //bool pcf8574_read(uint8_t* data){
eunmango 97:59d348745d96 135 // return i2c.read(PCF8574_ADDR, (char*) data, 1, 0) == 0;
eunmango 97:59d348745d96 136 //}
eunmango 97:59d348745d96 137 //
eunmango 97:59d348745d96 138 //int pcf8574_test(uint8_t value){
eunmango 97:59d348745d96 139 //
eunmango 97:59d348745d96 140 // int ret;
eunmango 97:59d348745d96 141 // uint8_t data=0;
eunmango 97:59d348745d96 142 //
eunmango 97:59d348745d96 143 // ret = pcf8574_write(value);
eunmango 97:59d348745d96 144 // if(!ret) return -1;
eunmango 97:59d348745d96 145 //
eunmango 97:59d348745d96 146 // ret = pcf8574_read(&data);
eunmango 97:59d348745d96 147 // if(!ret) return -2;
eunmango 97:59d348745d96 148 //
eunmango 97:59d348745d96 149 // return data;
eunmango 97:59d348745d96 150 //}
eunmango 97:59d348745d96 151 //
eunmango 97:59d348745d96 152 //void detected(void)
eunmango 97:59d348745d96 153 //{
eunmango 97:59d348745d96 154 // pcf8574_test(0xff);
eunmango 97:59d348745d96 155 // while(pcf8574_test(0xff)<255) pcf8574_test(0x00);
eunmango 97:59d348745d96 156 //}
eunmango 97:59d348745d96 157
eunmango 97:59d348745d96 158 int state=0; //0:forward / 1:left / 2:right
eunmango 97:59d348745d96 159 int cnt=0;
eunmango 97:59d348745d96 160 int period_us;
eunmango 97:59d348745d96 161 int beat_ms;
Jonathan Austin 0:2757d7abb7d9 162
eunmango 97:59d348745d96 163 void TRSensors(void) {
eunmango 97:59d348745d96 164
eunmango 97:59d348745d96 165 for(int i=0; i< NUMSENSORS ; i++) {
eunmango 97:59d348745d96 166 calibratedMin[i]= MAX;
eunmango 97:59d348745d96 167 calibratedMax[i] = MIN;
eunmango 97:59d348745d96 168 }
eunmango 97:59d348745d96 169
eunmango 97:59d348745d96 170 }
eunmango 97:59d348745d96 171
eunmango 97:59d348745d96 172 void set_TLC1543 (void) {
eunmango 97:59d348745d96 173
eunmango 97:59d348745d96 174 int value;
eunmango 97:59d348745d96 175 for(int i=0; i<6 ; i++){
eunmango 97:59d348745d96 176 cs=0;
eunmango 97:59d348745d96 177 wait_us(2);
eunmango 97:59d348745d96 178 value=spi.write(i<<12);
eunmango 97:59d348745d96 179 cs=1;
eunmango 97:59d348745d96 180 wait_us(21);
eunmango 97:59d348745d96 181 cs=0;
eunmango 97:59d348745d96 182 wait_us(2);
eunmango 97:59d348745d96 183 value=spi.write(i<<12);
eunmango 97:59d348745d96 184 //pc.printf("%d th : 0x%X\r\n", i, value);
eunmango 97:59d348745d96 185 cs=1;
eunmango 97:59d348745d96 186 wait_us(21);
eunmango 97:59d348745d96 187 sensor_values[i]=value;
eunmango 97:59d348745d96 188 }
eunmango 97:59d348745d96 189 }
eunmango 97:59d348745d96 190
eunmango 97:59d348745d96 191 void calibrate(void){
eunmango 97:59d348745d96 192 int i;
eunmango 97:59d348745d96 193
eunmango 97:59d348745d96 194 unsigned int max_sensor_values[NUMSENSORS];
eunmango 97:59d348745d96 195 unsigned int min_sensor_values[NUMSENSORS];
eunmango 97:59d348745d96 196
eunmango 97:59d348745d96 197 int j;
eunmango 97:59d348745d96 198 for(j=0; j <10 ; j++){
eunmango 97:59d348745d96 199 set_TLC1543();
eunmango 97:59d348745d96 200 for(i=0; i<NUMSENSORS; i++){
eunmango 97:59d348745d96 201 if(j==0 || max_sensor_values[i] < sensor_values[i]) max_sensor_values[i] = sensor_values[i];
eunmango 97:59d348745d96 202 if(j==0 || min_sensor_values[i] > sensor_values[i]) min_sensor_values[i] = sensor_values[i];
eunmango 97:59d348745d96 203 }
eunmango 97:59d348745d96 204 }
eunmango 97:59d348745d96 205
eunmango 97:59d348745d96 206 for(i=0; i<NUMSENSORS; i++){
eunmango 97:59d348745d96 207 if(min_sensor_values[i] > calibratedMax[i]) calibratedMax[i] = min_sensor_values[i];
eunmango 97:59d348745d96 208 if(max_sensor_values[i] < calibratedMin[i]) calibratedMin[i] = max_sensor_values[i];
eunmango 97:59d348745d96 209 }
eunmango 97:59d348745d96 210
eunmango 97:59d348745d96 211 }
Jonathan Austin 0:2757d7abb7d9 212
eunmango 97:59d348745d96 213 void readCalibrated(){
eunmango 97:59d348745d96 214 int i;
eunmango 97:59d348745d96 215 set_TLC1543();
eunmango 97:59d348745d96 216 for(i=0; i<NUMSENSORS ; i++){
eunmango 97:59d348745d96 217 //unsigned int calmin, calmax;
eunmango 97:59d348745d96 218 unsigned int denominator;
eunmango 97:59d348745d96 219 denominator = calibratedMax[i] - calibratedMin[i];
eunmango 97:59d348745d96 220 signed int x =0;
eunmango 97:59d348745d96 221 if(denominator !=0) {
eunmango 97:59d348745d96 222 x = ( ( (signed long)sensor_values[i] ) -calibratedMin[i]) * 1000 / denominator;
eunmango 97:59d348745d96 223 }
eunmango 97:59d348745d96 224 if(x<0) x=0;
eunmango 97:59d348745d96 225 else if(x>1000) x= 1000;
eunmango 97:59d348745d96 226 else sensor_values[i] = x;
eunmango 97:59d348745d96 227 }
eunmango 97:59d348745d96 228 }
mbed_official 88:bea4f2daa48c 229
eunmango 97:59d348745d96 230 int readLine() {
eunmango 97:59d348745d96 231 unsigned char i, on_line =0;
eunmango 97:59d348745d96 232 unsigned long avg;
eunmango 97:59d348745d96 233 unsigned int sum;
eunmango 97:59d348745d96 234 static int last_value=0;
eunmango 97:59d348745d96 235
eunmango 97:59d348745d96 236 readCalibrated();
eunmango 97:59d348745d96 237 avg =0; sum =0;
eunmango 97:59d348745d96 238 for(i=0; i< NUMSENSORS; i++){
eunmango 97:59d348745d96 239 int value = sensor_values[i];
eunmango 97:59d348745d96 240 value = 1000-value;
eunmango 97:59d348745d96 241 sensor_values[i]=value;
eunmango 97:59d348745d96 242 in_line[i]=FALS;
eunmango 97:59d348745d96 243 if(value > 300) {
eunmango 97:59d348745d96 244 on_line =1;
eunmango 97:59d348745d96 245 in_line[i]=TRU;
eunmango 97:59d348745d96 246 }
eunmango 97:59d348745d96 247 //if(value > calibratedMin[i]) on_line=1;
eunmango 97:59d348745d96 248 if(value > 50){
eunmango 97:59d348745d96 249 avg += (long)(value)*(i*1000);
eunmango 97:59d348745d96 250 sum += value;
eunmango 97:59d348745d96 251 }
eunmango 97:59d348745d96 252 }
eunmango 97:59d348745d96 253
eunmango 97:59d348745d96 254 if(!on_line){
eunmango 97:59d348745d96 255 if(last_value < (NUMSENSORS-1)*1000/2) return 0;
eunmango 97:59d348745d96 256 else return ( NUMSENSORS-1 ) * 1000;
eunmango 97:59d348745d96 257 }
eunmango 97:59d348745d96 258
eunmango 97:59d348745d96 259 last_value = avg/sum;
eunmango 97:59d348745d96 260
eunmango 97:59d348745d96 261 return last_value;
eunmango 97:59d348745d96 262
eunmango 97:59d348745d96 263 }
eunmango 97:59d348745d96 264
eunmango 97:59d348745d96 265 /****************************************************************
eunmango 97:59d348745d96 266 before running, set up tlc1543, calibrate **********************/
eunmango 97:59d348745d96 267 void settlc1543(){
eunmango 97:59d348745d96 268 cs =1;
eunmango 97:59d348745d96 269 spi.format(16,0);
eunmango 97:59d348745d96 270 spi.frequency(2000000);
eunmango 97:59d348745d96 271 pc.printf("test\r\n");
eunmango 97:59d348745d96 272 //pc.attach(callback(&rx_cb));
eunmango 97:59d348745d96 273
eunmango 97:59d348745d96 274 TRSensors();
eunmango 97:59d348745d96 275 for(int i=0; i<10; i++) {
eunmango 97:59d348745d96 276 calibrate();
eunmango 97:59d348745d96 277 wait(0.4);
eunmango 97:59d348745d96 278 }
eunmango 97:59d348745d96 279 // pc.printf("calibrate done\r\n");
eunmango 97:59d348745d96 280 for(int i=0; i<NUMSENSORS; i++) {
eunmango 97:59d348745d96 281 // pc.printf("Calibration Min: %d Max: %d\r\n", calibratedMin[i], calibratedMax[i] );
eunmango 97:59d348745d96 282 }
eunmango 97:59d348745d96 283 }
eunmango 97:59d348745d96 284
eunmango 97:59d348745d96 285
eunmango 97:59d348745d96 286 void rx_cb(void) {
eunmango 97:59d348745d96 287
eunmango 97:59d348745d96 288 int ch;
eunmango 97:59d348745d96 289 ch = pc.getc();
eunmango 97:59d348745d96 290 rx_buffer[index]=ch;
eunmango 97:59d348745d96 291 index++;
eunmango 97:59d348745d96 292 pc.putc(ch);
eunmango 97:59d348745d96 293 if(ch==0x0D) {
eunmango 97:59d348745d96 294 pc.putc(0x0A);
eunmango 97:59d348745d96 295 flag=1;
eunmango 97:59d348745d96 296 index=0;
eunmango 97:59d348745d96 297 }
eunmango 97:59d348745d96 298
eunmango 97:59d348745d96 299
eunmango 97:59d348745d96 300 }
eunmango 97:59d348745d96 301
eunmango 97:59d348745d96 302
mbed_official 82:abf1b1785bd7 303 int main()
mbed_official 82:abf1b1785bd7 304 {
eunmango 97:59d348745d96 305 clock_t t;
eunmango 97:59d348745d96 306 int f;
eunmango 97:59d348745d96 307 char displaySentence[40] = "Time is ";
eunmango 97:59d348745d96 308 char time[20];
eunmango 97:59d348745d96 309 irrecv.enableIRIn();
eunmango 97:59d348745d96 310 // mu.startUpdates();//start mesuring the distance
eunmango 97:59d348745d96 311 int data;
eunmango 97:59d348745d96 312
eunmango 97:59d348745d96 313 SSD1306 display = SSD1306();
eunmango 97:59d348745d96 314
eunmango 97:59d348745d96 315 while(1){
eunmango 97:59d348745d96 316
eunmango 97:59d348745d96 317 if(irrecv.decode(&result)){
eunmango 97:59d348745d96 318 pc.printf("%X\r\n",result.value);
eunmango 97:59d348745d96 319 irrecv.resume();
eunmango 97:59d348745d96 320
eunmango 97:59d348745d96 321 if(result.value == KEY1){ // 1
eunmango 97:59d348745d96 322 break;
eunmango 97:59d348745d96 323 }
eunmango 97:59d348745d96 324 if(result.value == KEY2){ // 2
eunmango 97:59d348745d96 325 IRForward();
eunmango 97:59d348745d96 326 }
eunmango 97:59d348745d96 327 if(result.value == KEY3){ // 3
eunmango 97:59d348745d96 328 CalibrationMotor();
eunmango 97:59d348745d96 329 settlc1543();
eunmango 97:59d348745d96 330
eunmango 97:59d348745d96 331 }
eunmango 97:59d348745d96 332 if(result.value == KEY6){ // 6
eunmango 97:59d348745d96 333 right();
mbed_official 82:abf1b1785bd7 334
eunmango 97:59d348745d96 335 }
eunmango 97:59d348745d96 336 if(result.value == KEY4){ // 4
eunmango 97:59d348745d96 337 left();
eunmango 97:59d348745d96 338 }
eunmango 97:59d348745d96 339 if(result.value == KEY8 ){ // 8
eunmango 97:59d348745d96 340 backward();
eunmango 97:59d348745d96 341 }
eunmango 97:59d348745d96 342 if(result.value == KEY5){ // 5
eunmango 97:59d348745d96 343 stop();
eunmango 97:59d348745d96 344 }
eunmango 97:59d348745d96 345 }
eunmango 97:59d348745d96 346
eunmango 97:59d348745d96 347 }
eunmango 97:59d348745d96 348
eunmango 97:59d348745d96 349
eunmango 97:59d348745d96 350
eunmango 97:59d348745d96 351 t=clock();
eunmango 97:59d348745d96 352 pc.printf("Hello PCF8574\n");
eunmango 97:59d348745d96 353 while (1) {
eunmango 97:59d348745d96 354
eunmango 97:59d348745d96 355 ultra_result = ultra.distance();
eunmango 97:59d348745d96 356 if(ultra_result > 0 && ultra_result < 14) {
eunmango 97:59d348745d96 357 if(in_line[0]==TRU && in_line[1]==TRU ){
eunmango 97:59d348745d96 358 stop();
eunmango 97:59d348745d96 359 mode = 2;
eunmango 97:59d348745d96 360 }
eunmango 97:59d348745d96 361
eunmango 97:59d348745d96 362 }
eunmango 97:59d348745d96 363
mbed_official 82:abf1b1785bd7 364
eunmango 97:59d348745d96 365 if(mode == 1){
eunmango 97:59d348745d96 366 pc.printf("enter\r\n");
eunmango 97:59d348745d96 367 position = (float)readLine();
eunmango 97:59d348745d96 368 pc.printf("position: %f\r\n", position);
eunmango 97:59d348745d96 369 float proportional =(float)(position-2000.0); //e(t)
eunmango 97:59d348745d96 370 float derivative = (float)(proportional - last_proportional);
eunmango 97:59d348745d96 371 integral += proportional;
eunmango 97:59d348745d96 372 last_proportional = proportional;//
eunmango 97:59d348745d96 373 //pid 15/60000/2 - 85%
eunmango 97:59d348745d96 374 float power_difference = proportional/16.0+ (float)integral/
eunmango 97:59d348745d96 375 60000.0+ derivative*2.0; //integral 변수 커지고 작아지는 값 확인하
eunmango 97:59d348745d96 376 pc.printf("proportional : %f integral %f derivative : %f \r\n", proportional, integral, derivative);
eunmango 97:59d348745d96 377 const int maximum = 255;
eunmango 97:59d348745d96 378 const int base = 155;
eunmango 97:59d348745d96 379 pc.printf("power_difference is %f\r\n" , power_difference);
eunmango 97:59d348745d96 380 if(power_difference > base ) power_difference = base;
eunmango 97:59d348745d96 381 if(power_difference < -base) power_difference = -base;
eunmango 97:59d348745d96 382
eunmango 97:59d348745d96 383
eunmango 97:59d348745d96 384
eunmango 97:59d348745d96 385 if(power_difference < 0){ //오른쪽으로 빠
eunmango 97:59d348745d96 386 // rightmotor.setSpeed((float)(base + power_difference) / (float)maximum);
eunmango 97:59d348745d96 387 // leftmotor.setSpeed((float)base-power_difference/(float)maximum);
eunmango 97:59d348745d96 388 pc.printf("right : left %f, right %f \r\n",(float)(base+power_difference) / (float)maximum , (float)(base) / (float)maximum );
eunmango 97:59d348745d96 389 rightmotor.setSpeed((float)(base) / (float)maximum);
eunmango 97:59d348745d96 390 leftmotor.setSpeed((float)(base+power_difference) / (float)maximum );
eunmango 97:59d348745d96 391 } else{// 왼//쪽으로 빠짐.
eunmango 97:59d348745d96 392 // rightmotor.setSpeed((float)(base + power_difference) / (float)maximum);
eunmango 97:59d348745d96 393 // leftmotor.setSpeed((float)base-power_difference/(float)maximum);
eunmango 97:59d348745d96 394 pc.printf("left : left %f, right %f \r\n",(float)(base) / (float)maximum , (float)(base -power_difference) / (float)maximum );
eunmango 97:59d348745d96 395 leftmotor.setSpeed((float)(base) / (float)maximum);
eunmango 97:59d348745d96 396 rightmotor.setSpeed((float)(base -power_difference) / (float)maximum);
eunmango 97:59d348745d96 397
eunmango 97:59d348745d96 398 }
eunmango 97:59d348745d96 399
eunmango 97:59d348745d96 400 if(in_line[0]&&in_line[1]&&in_line[2]&&in_line[3]&&in_line[4]){
eunmango 97:59d348745d96 401 leftmotor.setSpeed(0);
eunmango 97:59d348745d96 402 rightmotor.setSpeed(0);
eunmango 97:59d348745d96 403 pc.printf("stop\r\n");
eunmango 97:59d348745d96 404 //시간 모니터에 출력.
eunmango 97:59d348745d96 405 break;
eunmango 97:59d348745d96 406 }
eunmango 97:59d348745d96 407 }
eunmango 97:59d348745d96 408
eunmango 97:59d348745d96 409 if(mode == 2){
eunmango 97:59d348745d96 410 ultra_result = ultra.distance();
eunmango 97:59d348745d96 411
eunmango 97:59d348745d96 412 for(int i=0; i<50; i++){
eunmango 97:59d348745d96 413 data = readLine();
eunmango 97:59d348745d96 414 if(in_line[2]==TRU && in_line[1]==FALS && in_line[3]==FALS && in_line[0]==FALS && in_line[4]==FALS)
eunmango 97:59d348745d96 415 {
eunmango 97:59d348745d96 416 mode=1;
eunmango 97:59d348745d96 417 break;
eunmango 97:59d348745d96 418 }
eunmango 97:59d348745d96 419 ultraLeft();
eunmango 97:59d348745d96 420 }
eunmango 97:59d348745d96 421 }
Jonathan Austin 0:2757d7abb7d9 422 }
eunmango 97:59d348745d96 423 display.begin(true);
eunmango 97:59d348745d96 424 display.setTextSize(1);
eunmango 97:59d348745d96 425 display.setTextColor(WHITE);
eunmango 97:59d348745d96 426 display.setCursor(0,0);
eunmango 97:59d348745d96 427 t=clock()-t;
eunmango 97:59d348745d96 428 sprintf(time, "%f ms", (float)t*10);
eunmango 97:59d348745d96 429 strcat(displaySentence, time);
eunmango 97:59d348745d96 430 display.println(displaySentence);
eunmango 97:59d348745d96 431 display.display();
eunmango 97:59d348745d96 432 }