radcon.cpp

#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <chrono>

#include <wiringPi.h>
using namespace std;

auto start_time_msec = chrono::system_clock::now();

const int led_right = 25;
const int led_left = 11;
bool isRightLedOn = false;
bool isLeftLedOn = false;
int high_or_low = 0; //LEDがハイかローかを判定-> 0 low, 1 high

const int motor1Reg = 22; //モーター1を正方向に回すピン
const int motor1Rev = 17; //モーター1を逆方向に回すピン
bool isMotor1RegOn = false;
bool isMotor1RevOn = false;

const int motor2Reg = 24; //モーター2を正方向に回すピン
const int motor2Rev = 23; //モーター2を逆方向に回すピン
bool isMotor2RegOn = false;
bool isMotor2RevOn = false;

void move(char*);
void refresh();
void blink_led();

int main(void){
    char bin[8];
    bool quit = false;
    string line;

    wiringPiSetupGpio();
    pinMode(led_right, OUTPUT);
    pinMode(led_left, OUTPUT);
    pinMode(motor1Reg, OUTPUT);
    pinMode(motor1Rev, OUTPUT);
    pinMode(motor2Reg, OUTPUT);
    pinMode(motor2Rev, OUTPUT);
    ifstream ifs("/dev/input/js0", ios::binary);
    if(ifs.fail()){
        cerr << "Reading file is fail." << endl;
        return 1;
    }
    for(int i = 0;i < 50;i++){
        ifs.read((char*)bin, sizeof(char)*8);
    }

    while(!ifs.eof()){
        ifs.read((char*)bin, sizeof(char)*8);
        move(bin);
        if(isRightLedOn || isLeftLedOn)
            blink_led();
    }
    ifs.close();

    return 0;
}

void move(char* c){
    if(c[4] == 0x01){
        switch(c[7]){
            case 0x04: digitalWrite(motor1Reg, HIGH);
                        digitalWrite(motor2Reg, HIGH);
                        isMotor1RegOn = isMotor2RegOn = true;
                        break;
            case 0x05: if(isMotor1RegOn && isMotor2RegOn){
                            digitalWrite(motor1Reg, LOW);
                            isMotor1RegOn = false;
                        }else if(isMotor1RevOn && isMotor2RevOn){
                            digitalWrite(motor1Rev, LOW);
                            isMotor1RevOn = false;
                        }else{
                            refresh();
                            digitalWrite(motor1Rev, HIGH);
                            digitalWrite(motor2Reg, HIGH);
                            isMotor1RevOn = true;
                            isMotor2RegOn = true;
                        }
                        break;
            case 0x06: digitalWrite(motor1Rev, HIGH);
                        digitalWrite(motor2Rev, HIGH);
                        isMotor1RevOn = isMotor2RevOn = true;
                        break;
            case 0x07: if(isMotor1RegOn && isMotor2RegOn){
                            digitalWrite(motor2Reg, LOW);
                            isMotor2RegOn = false;
                        }else if(isMotor1RevOn && isMotor2RevOn){
                            digitalWrite(motor2Rev, LOW);
                            isMotor2RevOn = false;
                        }else{
                            refresh();
                            digitalWrite(motor2Rev, HIGH);
                            digitalWrite(motor1Reg, HIGH);
                            isMotor2RevOn = true;
                            isMotor1RegOn = true;
                        }
                        break;
            case 0x0A: if(isRightLedOn){
                            digitalWrite(led_right, LOW);
                            isRightLedOn = false;
                        }
                        isLeftLedOn = true;
                        high_or_low = 1;
                        start_time_msec = chrono::system_clock::now();
                        break;
            case 0x0B: if(isLeftLedOn){
                            digitalWrite(led_left, LOW);
                            isLeftLedOn = false;
                        }
                        isRightLedOn = true;
                        high_or_low = 1;
                        start_time_msec = chrono::system_clock::now();
                        break;
        }
    }else if(c[4] == 0x00){
        switch(c[7]){
            case 0x04: digitalWrite(motor1Reg, LOW);
                        digitalWrite(motor2Reg, LOW);
                        isMotor1RegOn = isMotor2RegOn = false;
                        break;
            case 0x05: if(isMotor2RegOn && isMotor1RevOn){
                            digitalWrite(motor1Rev, LOW);
                            digitalWrite(motor2Reg, LOW);
                            isMotor1RevOn = isMotor2RegOn = false;
                        }else if(isMotor2RegOn){
                            digitalWrite(motor1Reg, HIGH);
                            isMotor1RegOn = true;
                        }else if(isMotor2RevOn){
                            digitalWrite(motor1Rev, HIGH);
                            isMotor1RevOn = true;
                        }
                        break;
            case 0x06: digitalWrite(motor1Rev, LOW);
                        digitalWrite(motor2Rev, LOW);
                        isMotor1RevOn = isMotor2RevOn = false;
                        break;
            case 0x07: if(isMotor1RegOn && isMotor2RevOn){
                            digitalWrite(motor1Reg, LOW);
                            digitalWrite(motor2Rev, LOW);
                            isMotor1RegOn = isMotor2RevOn = false;
                        }else if(isMotor1RegOn){
                            digitalWrite(motor2Reg, HIGH);
                            isMotor2RegOn = true;
                        }else if(isMotor1RevOn){
                            digitalWrite(motor2Rev, HIGH);
                            isMotor2RevOn = true;
                        }
                        break;
            case 0x0A: if(isLeftLedOn){
                            digitalWrite(led_left, LOW);
                            isLeftLedOn = false;
                            high_or_low = 0;
                        }else{}
                        break;
            case 0x0B: if(isRightLedOn){
                            digitalWrite(led_right, LOW);
                            isRightLedOn = false;
                            high_or_low = 0;
                        }else{}
                        break;
        }
    }
}

void refresh(){
    digitalWrite(motor1Reg, LOW);
    digitalWrite(motor1Rev, LOW);
    digitalWrite(motor2Reg, LOW);
    digitalWrite(motor2Rev, LOW);
    isMotor1RegOn = isMotor1RevOn = isMotor2RegOn = isMotor2RevOn = false;
}

void blink_led(){
    auto now_time = chrono::system_clock::now();
    auto dur = chrono::duration_cast<chrono::milliseconds>(now_time - start_time_msec).count();
    if((int)dur > 300){
        high_or_low = (high_or_low+1)%2;
        if(isRightLedOn){
            if(high_or_low == 1)
                digitalWrite(led_right, HIGH);
            else
                digitalWrite(led_right, LOW);
            start_time_msec = now_time;
        }else if(isLeftLedOn){
            if(high_or_low == 1)
                digitalWrite(led_left, HIGH);
            else
                digitalWrite(led_left, LOW);
            start_time_msec = now_time;
        }
    }
}