// Micon-Untendai-27
// 2021.4.17
// Arduino UNO を使用する。

#include <LiquidCrystal.h>
#define  JOV   A0
#define  MS1   15
#define  MS2   16
#define  MS4   17
#define  BK1   18
#define  BK2   19
#define  BK4   8
#define  SYA1  11
#define  SYA2  12
#define  SYA4  13
#define  PWM   10

LiquidCrystal lcd(7,6,5,4,3,2);

  int v;
  int jov;
  int brake;
  int mascn;
  int vm;
  int syasy;
  int dt;

void setup() {
  lcd.begin(16,2);
  Serial.begin(9600);
  pinMode(JOV,INPUT);
  pinMode(MS1,INPUT);
  pinMode(MS2,INPUT);
  pinMode(MS4,INPUT);
  pinMode(BK1,INPUT);
  pinMode(BK2,INPUT);
  pinMode(BK4,INPUT);
  pinMode(SYA1,INPUT);
  pinMode(SYA2,INPUT);
  pinMode(SYA4,INPUT);
  pinMode(PWM, OUTPUT);
  v = 0;

  TCCR1A = 0b01100011;
  TCCR1B = 0b00011010;
  OCR1A = 99;
}

void loop(){

//ブレーキスイッチをチェックする

  if (digitalRead(BK1)==LOW && digitalRead(BK2)==LOW && digitalRead(BK4)==LOW){
    brake = 0;
  } else if (digitalRead(BK1)==HIGH && digitalRead(BK2)==LOW && digitalRead(BK4)==LOW){
    brake = 1;
  } else if (digitalRead(BK1)==LOW && digitalRead(BK2)==HIGH && digitalRead(BK4)==LOW){
    brake = 2;
  } else if (digitalRead(BK1)==HIGH && digitalRead(BK2)==HIGH && digitalRead(BK4)==LOW){
    brake = 3;
  } else if (digitalRead(BK1)==LOW && digitalRead(BK2)==LOW && digitalRead(BK4)==HIGH){
    brake = 4;
  } else if (digitalRead(BK1)==HIGH && digitalRead(BK2)==LOW && digitalRead(BK4)==HIGH){
    brake = 5;
  } else if (digitalRead(BK1)==LOW && digitalRead(BK2)==HIGH && digitalRead(BK4)==HIGH){
    brake = 6;
  } else if (digitalRead(BK1)==HIGH && digitalRead(BK2)==HIGH && digitalRead(BK4)==HIGH){
    brake = 7;
  }
  
//マスコンスイッチをチェックする
    if (digitalRead(MS1)==HIGH && digitalRead(MS2)==LOW && digitalRead(MS4)==LOW){
      mascn = 1;
      vm = 60;
    } else if (digitalRead(MS1)==LOW && digitalRead(MS2)==HIGH && digitalRead(MS4)==LOW){
      mascn = 2;
      vm = 80;
    } else if (digitalRead(MS1)==HIGH && digitalRead(MS2)==HIGH && digitalRead(MS4)==LOW){
      mascn = 3;
      vm = 100;
    } else if (digitalRead(MS1)==LOW && digitalRead(MS2)==LOW && digitalRead(MS4)==HIGH){
      mascn = 4;
      vm = 120;
    } else {
      mascn = 0;
      vm = 0;
    }

//車種スイッチをチェックする
  if (digitalRead(SYA1)==LOW && digitalRead(SYA2)==LOW && digitalRead(SYA4)==LOW){
    syasy = 0;
  } else if (digitalRead(SYA1)==HIGH && digitalRead(SYA2)==LOW && digitalRead(SYA4)==LOW){
    syasy = 1;  
  } else if (digitalRead(SYA1)==LOW && digitalRead(SYA2)==HIGH && digitalRead(SYA4)==LOW){
    syasy = 2;
  } else if (digitalRead(SYA1)==HIGH && digitalRead(SYA2)==HIGH && digitalRead(SYA4)==LOW){
    syasy = 3;
  } else if (digitalRead(SYA1)==LOW && digitalRead(SYA2)==LOW && digitalRead(SYA4)==HIGH){
    syasy = 4;
  } else if (digitalRead(SYA1)==HIGH && digitalRead(SYA2)==LOW && digitalRead(SYA4)==HIGH){
    syasy = 5;
  } else if (digitalRead(SYA1)==LOW && digitalRead(SYA2)==HIGH && digitalRead(SYA4)==HIGH){
    syasy = 6;
  } else if (digitalRead(SYA1)==HIGH && digitalRead(SYA2)==HIGH && digitalRead(SYA4)==HIGH){
    syasy = 7;   
  }

  
//演算の実施  
  if (brake == 0 ){   
    if (mascn == 0 ){      //惰行走行中
      v = v - 1;
      if (v<0){
        v=0;
      }
    } else if (vm>v){            //力行操作中
      v = v + 0.1*(vm+9-v);
    } else {
      v = v + 0.1*(vm-v);
    }
  } else if (v > 0){  //制動操作中
      v = v - 2*brake;
      if (v<0){
        v=0;
      }
  } 

//Duty値を計算する
  if (syasy == 0){
      dt = 1.59*v;    
  } else if (syasy == 1){
      dt = 1.23*v;
  } else if (syasy == 2){
      dt = 0.94*v;
  } else if (syasy == 3){
      dt = 0.72*v;
  } else if (syasy == 4){
      dt = 0.60*v;
  } else if (syasy >= 5){
      dt = 0.47*v;
  } 

//常灯レベルを読む  
  jov = analogRead(JOV);    
  jov = jov*0.04;
  if (dt <= jov ){
    dt = jov;
  }

//PWM出力
  analogWrite(PWM,dt);
  
//液晶ディスプレイ表示
  char out_str1[16];
  sprintf(out_str1,"MASCN %d  BRAKE %d",mascn,brake);
  lcd.setCursor (0,0);
  lcd.print(out_str1);
  sprintf(out_str1,"SPEED %d Km/h     ",v);
  lcd.setCursor (0,1);
  lcd.print(out_str1);

   
//  String buf = String(digitalRead(SYA1))+","+String(digitalRead(SYA2))+","+String(digitalRead(SYA4))+",SYASYU="+String(syasy)+","+
//  String(digitalRead(MS1))+","+String(digitalRead(MS2))+","+String(digitalRead(MS4))+",MASCN="+String(mascn)+",MAX="+String(vm)+","+
//  String(digitalRead(BK1))+","+String(digitalRead(BK2))+","+String(digitalRead(BK4))+",BRAKE="+String(brake);
//  Serial.println(buf);

//  Serial.print("SPEED= ");
//  Serial.print(v);
//  Serial.print(" Km/h  "); 
//  Serial.print("Duty= "); 
//  Serial.println(dt); 

  delay(500);
  
}