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

dd

Dependencies:   Final HCSR04 TB6612FNG

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

Committer:
eunmango
Date:
Sun Jun 16 04:44:35 2019 +0000
Revision:
97:b483e656bd14
Parent:
88:bea4f2daa48c 
t

Who changed what in which revision?

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