6차시

문제점

To Do

#include <wiringPi.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <unistd.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>

#define I2C_DEVICE "/dev/i2c-1"
#define I2C_ADDR 0x16

#define Tracking_Left1 2;
#define Tracking_Left2 3;
#define Tracking_Right2 0;
#define Tracking_Right1 7; 

int fd;

int open_i2c() {
    if ((fd = open(I2C_DEVICE, O_RDWR)) < 0) {
        printf("Failed to open the i2c bus\n");
        return -1;
    }
    if (ioctl(fd, I2C_SLAVE, I2C_ADDR) < 0) {
        printf("Failed to access addr\n");
        return -1;
    }
    return 0;
}

void ctrl_car(int l_dir, int l_speed, int r_dir, int r_speed) {
    int reg = 0x01;
    unsigned char data[5] = {reg, (unsigned char)l_dir, (unsigned char)l_speed, (unsigned char)r_dir, (unsigned char)r_speed};
    if (write(fd, data, 5) != 5) {
        printf("Failed to write to the i2c bus\n");
    }
}

void Car_Left(int speed1, int speed2) {
    Ctrl_Car(car, 0, speed1, 1, speed2);
}

void Car_Right(int speed1, int speed2) {
    Ctrl_Car(car, 1, speed1, 0, speed2);
}

void Car_Spin_Left(int speed1, int speed2) {
    Ctrl_Car(car, 0, speed1, 1, speed2);
}

void Car_Spin_Right(int speed1, int speed2) {
    Ctrl_Car(car, 1, speed1, 0, speed2);
}

void car_run(int speed1, int speed2) {
    ctrl_car(1, speed1, 1, speed2);
}

void car_stop() {
    int reg = 0x02;
    unsigned char data[2] = {reg, 0x00};
    if (write(fd, data, 2) == -1) {
        printf("Failed to write to the i2c bus\n");
    } 
    else printf ("car stop"); 
}

int main() {
    if (open_i2c() != 0) {
        return -1;
    }
    if (wiringPiSetup() == -1) {
        printf("WiringPiSetup fail");
        return -1;
    }

    pinMode(Tracking_Left1, INPUT);
    pinMode(Tracking_Left2, INPUT);
    pinMode(Tracking_Right1, INPUT);
    pinMode(Tracking_Right2, INPUT);
    
    while (1) {
        if (digitalRead(Tracking_Left2) == 0 && digitalRead(Tracking_Right1) == 0) {        
            car_run(100, 100);    
            printf("%d", digitalRead(Tracking_Left2));
            printf("%d", digitalRead(Tracking_Right1));
        }
        if (digitalRead(Tracking_Left2) == 0 && digitalRead(Tracking_Right1) == 1) {        
            Car_Spin_Left(30, 70);  
            printf("%d", digitalRead(Tracking_Left2));
            printf("%d", digitalRead(Tracking_Right1));
        }
        if (digitalRead(Tracking_Left2) == 1 && digitalRead(Tracking_Right1) == 0) {        
            Car_Spin_Left(70, 30);  
            printf("%d", digitalRead(Tracking_Left2));
            printf("%d", digitalRead(Tracking_Right1));
        } 
        if (digitalRead(Tracking_Left2) == 0 && digitalRead(Tracking_Right1) == 0) {        
            Car_Spin_Left(0, 0);  
            printf("%d", digitalRead(Tracking_Left2));
            printf("%d", digitalRead(Tracking_Right1));
        } 
    }

    return 0;
}