/*
  いずみ研的魔改造ミニ四駆 ～ AI制御に向けてのサンプル＆テンプレート
  立命館大学 理工学部 電子情報工学科 いずみ研

  https://www.ritsumei.ac.jp/se/re/izumilab/dist/mini4WD/index.html
  https://www.ritsumei.ac.jp/se/re/izumilab/dist/mini4WD/mini4WD2sample.pdf
  https://www.ritsumei.ac.jp/se/re/izumilab/dist/mini4WD/mini4WD2sample.zip

  SparkFun IMU Breakout - MPU-9250(+AK8963)
  I2C connected to A4(SDA)&A5(SCL)
*/

#include <Wire.h>
#include "mini4WD2.h"
#include "soundcode.h"

unsigned char run=0;
unsigned char status=0,brk=0,mtr=255;
unsigned long t,t0,t1,t_lastprint;


#define I2CAG 0x68
#define I2CM  0x0c
#define I2CTIMEOUTus  20
#define I2CTIMEOUTnum  5
#define I2CBUFSZ 16
unsigned char i2cbuf[I2CBUFSZ];

// プッシュスイッチ、スライドスイッチ×２、ボリューム、光センサ
int psw,ssw,ssw2,vol,pd;
// 加速度、角速度、磁気、温度
int acl[3], gyr[3], mag[3], thm;
// 発光ダイオード
unsigned char ld=0,ld2=0;

#define SNDERR01 0x40b
#define SNDERR02 0x40b0
#define SNDERR03 0x40b00
#define SNDERR04 0x40b000
#define SNDERR05 0x40b0000
#define SNDERR06 0x40b00000
#define SNDERR07 0x40bd
#define SNDERR08 0x40bdc

int byteHL2int(unsigned char h, unsigned char l){
  int v=0;
  v=(h&0x80)?-1:0;
  v=(v<<8)|h;
  v=(v<<8)|l;
  return v;
}

unsigned char I2CRead(int dadr, int radr){
  int t=I2CTIMEOUTnum;
  Wire.beginTransmission(dadr);
  Wire.write(radr);
  Wire.endTransmission(false);
  Wire.requestFrom(dadr,1);
  while (!Wire.available()) {
    delayMicroseconds(I2CTIMEOUTus);
    if (t--<=0) return 0xff;
  }
  return Wire.read();
}

int I2CReadN(int dadr, int radr, unsigned char *buf, int n){
  int i=0,t=I2CTIMEOUTnum;
  if (n<=0) goto _RETURN;
  Wire.beginTransmission(dadr);
  Wire.write(radr);
  Wire.endTransmission(false);
  Wire.requestFrom(dadr,n);
  while (i<n) {
    while (!Wire.available()) {
      delayMicroseconds(I2CTIMEOUTus);
      if (t--<=0) goto _RETURN;
    }
    buf[i++]=Wire.read();
  }
 _RETURN:
  buf[i]=0;
  return i;
}

void I2CWrite(int dadr, int radr, unsigned char val){
  Wire.beginTransmission(dadr);
  Wire.write(radr);
  Wire.write(val);
  Wire.endTransmission();
}

void I2CPrint(int dadr, int radr, char *msg){
  int v;
  Serial.print(msg);
  v=I2CRead(dadr,radr);
  Serial.println(v,HEX);
}

void setup() {
  pinMode(PinLD, OUTPUT);
  pinMode(PinLD2,OUTPUT);
  pinMode(PinBZ, OUTPUT);
  pinMode(PinMTR,OUTPUT);
  pinMode(PinBRK,OUTPUT);
  pinMode(PinBTN,INPUT);
  pinMode(PinSL, INPUT);
  pinMode(PinSL2,INPUT);

  digitalWrite(PinBRK, 0);
  digitalWrite(PinMTR, 1);
  Serial.begin(115200);
  Serial.print("\nRizm mini4WDsample ver.2025.11.20a\n");
  randomSeed(analogRead(PinPD));
  
  soundcode_pin   = 11;
  soundcode_tempo = 25;
  
  Wire.begin();
  /*
  I2CPrint(I2CAG,0x75,"75 Who am I  ");
  I2CPrint(I2CAG,0x6A,"6A User Ctrl ");
  I2CPrint(I2CAG,0x6B,"6B Pwr Mgmt1 ");
  I2CPrint(I2CAG,0x6C,"6C Pwr Mgmt2 ");
  I2CPrint(I2CAG,0x1A,"1A Config    ");
  I2CPrint(I2CAG,0x1B,"1B Gyro Cnf  ");
  I2CPrint(I2CAG,0x1C,"1C Accl Cnf  ");
  I2CPrint(I2CAG,0x1D,"1D Accl Cnf2 ");
  I2CPrint(I2CAG,0x37,"37 IntPinCfg ");
  I2CPrint(I2CM, 0x0a,"0A Mag Ctrl1 ");
  */
  I2CWrite(I2CAG,0x6B,0); // PwrMgmt1:wakeup
  I2CWrite(I2CAG,0x1B,0x18); // GyroCnf:2000dps,F=0 -> 2000dps/2^15=0.061dps/LSB
  I2CWrite(I2CAG,0x1C,0x18); // AcclCnf:16G -> 16G/2^15=0.00049G/LSB
  I2CWrite(I2CAG,0x37,0x02); // IntPinCfg:Bypass for Magnetic Sensor
  I2CWrite(I2CM, 0x0a,0x16); // MagCtrl1:16bit,100Hz -> 4800uT/2^15=0.15uT/LSB
  
  //soundcode_pin=13; // to mute sound

  soundcode_play(soundcode_f1gp);

  // wait for push button
  while (digitalRead(PinBTN)){
    digitalWrite(PinLD, 1);
    delay(50);
    digitalWrite(PinLD, 0);
    delay(50);
  }

  Serial.print("Ready ...\n");
  soundcode_play(soundcode_go);
  Serial.print("Go!\n");
  t0=t1=t_lastprint=millis();
}

// communication mode: non-blocking, binary
void loop() {
  t=millis();
  psw=digitalRead(PinBTN);
  ssw=digitalRead(PinSL);
  ssw2=digitalRead(PinSL2);
  vol=analogRead(PinVOL);
  pd=analogRead(PinPD);
    
  if (I2CReadN(I2CM,0x02,i2cbuf,8)!=8) {
    status|=0x10;
    digitalWrite(PinLD2,HIGH);
    soundcode_playword(SNDERR07);
  }
  if ((i2cbuf[0]&0x01)==0x00) {//data not ready
    status|=0x40;
    digitalWrite(PinLD2,HIGH);
  }
  if ((i2cbuf[7]&0x08)==0x08) {//over flow
    status|=0x80;
    digitalWrite(PinLD2,HIGH);
  }
  mag[0]=byteHL2int(i2cbuf[2],i2cbuf[1]);
  mag[1]=byteHL2int(i2cbuf[4],i2cbuf[3]);
  mag[2]=byteHL2int(i2cbuf[6],i2cbuf[5]);
  
  // Acceleration(XYZ), Temperature, AngularVelocity(XYZ)
  if (I2CReadN(I2CAG,0x3B,i2cbuf,14)!=14) {
    status|=0x10;
    digitalWrite(PinLD2,HIGH);
    soundcode_playword(SNDERR08);
  }
  acl[0]=byteHL2int(i2cbuf[ 0],i2cbuf[ 1]);
  acl[1]=byteHL2int(i2cbuf[ 2],i2cbuf[ 3]);
  acl[2]=byteHL2int(i2cbuf[ 4],i2cbuf[ 5]);
  thm   =byteHL2int(i2cbuf[ 6],i2cbuf[ 7]);
  gyr[0]=byteHL2int(i2cbuf[ 8],i2cbuf[ 9]);
  gyr[1]=byteHL2int(i2cbuf[10],i2cbuf[11]);
  gyr[2]=byteHL2int(i2cbuf[12],i2cbuf[13]);

  if ((t-t_lastprint)>1000) {
    // センサー値送信／1000ms以上の間隔で
    Serial.print(" PSW:");
    Serial.print(psw);
    Serial.print(" SSW:");
    Serial.print(ssw);
    Serial.print(" ");
    Serial.print(ssw2);
    Serial.print(" VOL:");
    Serial.print(vol);
    Serial.print(" PD:");
    Serial.print(pd);
    Serial.print(" ACL:");
    Serial.print(acl[0]);
    Serial.print(" ");
    Serial.print(acl[1]);
    Serial.print(" ");
    Serial.print(acl[2]);
    Serial.print(" GYR:");
    Serial.print(gyr[0]);
    Serial.print(" ");
    Serial.print(gyr[1]);
    Serial.print(" ");
    Serial.print(gyr[2]);
    Serial.print(" MAG:");
    Serial.print(mag[0]);
    Serial.print(" ");
    Serial.print(mag[1]);
    Serial.print(" ");
    Serial.print(mag[2]);
    Serial.print(" THM:");
    Serial.print(thm);
    Serial.println();
    t_lastprint=t;
  }

  // 利用可能な変数
  // t,t1 ... 現在の時刻[ms]、前回の時刻[ms]
  // psw,ssw,ssw2 ... プッシュボタン、スライドスイッチの値 (0=ON, 1=OFF)
  // ld, ld2 ... 発光ダイオードの値 (0=OFF, 1=ON)
  // vol ... 回転抵抗の値（左が小、右が大）
  // pd ... 路面光センサの値（黒が大きい、白が小さい）
  // acl[3], gyr[3], mag[3], thm ... 加速度、角速度、磁気、温度

  
  // 初心者向け改造箇所 -- ここから

   
  // モーター出力は値0で100%、値255で0%
  if (ssw==1) {
    analogWrite(PinMTR,255); // PWM 0%
  } else if (pd>800) {
    //analogWrite(PinMTR, 255); // PWM 0%
    //delay(2000);
    analogWrite(PinMTR,0); // PWM 100%
  } else {
    analogWrite(PinMTR,0); // PWM 100%
  }

  
  // 初心者向け改造箇所 -- ここまで

  digitalWrite(PinLD, ld);
  digitalWrite(PinLD, ld2);
  t1=t;

  delay(20); // 20ms以下にしないこと（センサが間に合わない）
}
