Электромотоцикл с гироскопом и регенерацией

 X=4 Y=2 Z=6 F=24ЯC
 X=4 Y=2 Z=6 F=24ЯC
 X=4 Y=2 Z=6 F=24ЯC
 X=4 Y=2 Z=6 F=24ЯC
 X=4 Y=2 Z=6 F=24ЯC
 X=3 Y=0 Z=6 F=24ЯC
 X=0 Y=-19 Z=21 F=24ЯC
 X=-31 Y=-159 Z=10 F=24ЯC
 X=-62 Y=-307 Z=-3 F=24ЯC
 X=-50 Y=-237 Z=5 F=24ЯC
 X=-61 Y=-227 Z=-1 F=24ЯC
 X=-77 Y=-221 Z=-6 F=24ЯC
 X=-85 Y=-223 Z=-4 F=24ЯC
 X=-40 Y=-190 Z=0 F=24ЯC
 X=22 Y=-94 Z=2 F=24ЯC
 X=8 Y=-34 Z=3 F=24ЯC
 X=2 Y=-7 Z=5 F=24ЯC
 X=-1 Y=-2 Z=6 F=24ЯC
 X=11 Y=-8 Z=4 F=24ЯC
 X=21 Y=-9 Z=2 F=24ЯC
 X=17 Y=-9 Z=3 F=24ЯC
 X=14 Y=-8 Z=2 F=24ЯC
 X=6 Y=-1 Z=5 F=24ЯC
 X=5 Y=0 Z=5 F=24ЯC
 X=7 Y=0 Z=5 F=24ЯC

#include <Wire.h> // I2C library, gyroscope
// Gyroscope ITG3200 
#define GYRO 0x68 //  when AD0 is connected to GND ,gyro address is 0x68.
//#define GYRO 0x69   when AD0 is connected to VCC ,gyro address is 0x69  
#define G_SMPLRT_DIV 0x15
#define G_DLPF_FS 0x16
#define G_INT_CFG 0x17
#define G_PWR_MGM 0x3E
#define G_TO_READ 8 // 2 bytes for each axis x, y, z

#define DELAY_TIME 8

// Назначим пины
// Troyka-Stepper:
// Шаг двигателя
#define STEP 7 
// Направление вращения
#define DIR  8 
// Включение двигателя
#define EN   11 
// offsets are chip specific. 
int g_offx = 120;
int g_offy = 20;
int g_offz = 93;
int hx, hy, hz, turetemp;
int oldvl,tmp;
//initializes the gyroscope
void initGyro()
{
 /*****************************************
 * ITG 3200
 * power management set to:
 * clock select = internal oscillator
 * no reset, no sleep mode
 * no standby mode
 * sample rate to = 125Hz
 * parameter to +/- 2000 degrees/sec
 * low pass filter = 5Hz
 * no interrupt
 ******************************************/
 writeTo(GYRO, G_PWR_MGM, 0x00);
 writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
 writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
 writeTo(GYRO, G_INT_CFG, 0x00);
}
void getGyroscopeData(int * result)
{
 /**************************************
 Gyro ITG-3200 I2C
 registers:
 temp MSB = 1B, temp LSB = 1C
 x axis MSB = 1D, x axis LSB = 1E
 y axis MSB = 1F, y axis LSB = 20
 z axis MSB = 21, z axis LSB = 22
 *************************************/
 int regAddress = 0x1B;
 int temp, x, y, z;
 byte buff[G_TO_READ];
 readFrom(GYRO, regAddress, G_TO_READ, buff); //read the gyro data from the ITG3200
 result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
 result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
 result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
 result[3] = (buff[0] << 8) | buff[1]; // temperature
 }
 //
void setup()
{
 Serial.begin(9600);
 Wire.begin();
 initGyro();
 // Настроим все необходимые пины на выход
  pinMode(STEP, OUTPUT);
  pinMode(DIR, OUTPUT);
  pinMode(EN, OUTPUT);
  oldvl=0;
}
//
void loop()
{
 byte addr;
 int gyro[4];
 int i;
 getGyroscopeData(gyro);
 hx = gyro[0] / 14.375;
 hy = gyro[1] / 14.375;
 hz = gyro[2] / 14.375;
 turetemp = 35+ ((double) (gyro[3] + 13200)) / 280; // temperature
 Serial.print(" X=");
 Serial.print(hx);
 Serial.print(" Y=");
 Serial.print(hy);
 Serial.print(" Z=");
 Serial.print(hz);
 Serial.print(" F=");
 Serial.print(turetemp);
 Serial.print((char)223);
 Serial.println("C");
 delay(50); 
 
 if(oldvl<hx){
   tmp=abs(hx)-abs(oldvl);
   oldvl=hx;
   digitalWrite(EN, HIGH);
   digitalWrite(DIR, HIGH);
   if(tmp<=0)tmp=1;
   for(i=0;i<tmp;i++){
      digitalWrite(STEP, HIGH);
      delay(DELAY_TIME);
      digitalWrite(STEP, LOW);
      delay(DELAY_TIME);
   }
   digitalWrite(EN, LOW);
 }
 
 if(oldvl>hx){
   tmp=abs(oldvl)-abs(hx);
   oldvl=hx;
   digitalWrite(EN, HIGH);
   digitalWrite(DIR, LOW);
   if(tmp<=0)tmp=1;
   for(i=0;i<tmp;i++){
      digitalWrite(STEP, HIGH);
      delay(DELAY_TIME);
      digitalWrite(STEP, LOW);
      delay(DELAY_TIME);
   }
   digitalWrite(EN, LOW);
 }
 
 
}
//---------------- Functions
//Writes val to address register on ACC
void writeTo(int DEVICE, byte address, byte val) {
  Wire.beginTransmission(DEVICE); //start transmission to ACC 
  Wire.write(address);        // send register address
  Wire.write(val);        // send value to write
  Wire.endTransmission(); //end transmission
}
//reads num bytes starting from address register on ACC in to buff array
 void readFrom(int DEVICE, byte address, int num, byte buff[]) {
 Wire.beginTransmission(DEVICE); //start transmission to ACC 
 Wire.write(address);        //sends address to read from
 Wire.endTransmission(); //end transmission
 
 Wire.beginTransmission(DEVICE); //start transmission to ACC
 Wire.requestFrom(DEVICE, num);    // request 6 bytes from ACC
 
 int i = 0;
 while(Wire.available())    //ACC may send less than requested (abnormal)
 { 
   buff[i] = Wire.read(); // receive a byte
   i++;
 }
 Wire.endTransmission(); //end transmission
}

#include <Wire.h> // I2C library, gyroscope
// Gyroscope ITG3200 
#define GYRO 0x68 //  when AD0 is connected to GND ,gyro address is 0x68.
//#define GYRO 0x69   when AD0 is connected to VCC ,gyro address is 0x69  
#define G_SMPLRT_DIV 0x15
#define G_DLPF_FS 0x16
#define G_INT_CFG 0x17
#define G_PWR_MGM 0x3E
#define G_TO_READ 8 // 2 bytes for each axis x, y, z

#define DELAY_TIME 8

// Назначим пины
// Troyka-Stepper:
// Шаг двигателя
#define STEP 7 
// Направление вращения
#define DIR  8 
// Включение двигателя
#define EN   11 
// offsets are chip specific. 
int g_offx = 120;
int g_offy = 20;
int g_offz = 93;
int hx, hy, hz, turetemp;
int oldvl,tmp;
//initializes the gyroscope
void initGyro()
{
 /*****************************************
 * ITG 3200
 * power management set to:
 * clock select = internal oscillator
 * no reset, no sleep mode
 * no standby mode
 * sample rate to = 125Hz
 * parameter to +/- 2000 degrees/sec
 * low pass filter = 5Hz
 * no interrupt
 ******************************************/
 writeTo(GYRO, G_PWR_MGM, 0x00);
 writeTo(GYRO, G_SMPLRT_DIV, 0x07); // EB, 50, 80, 7F, DE, 23, 20, FF
 writeTo(GYRO, G_DLPF_FS, 0x1E); // +/- 2000 dgrs/sec, 1KHz, 1E, 19
 writeTo(GYRO, G_INT_CFG, 0x00);
}
void getGyroscopeData(int * result)
{
 /**************************************
 Gyro ITG-3200 I2C
 registers:
 temp MSB = 1B, temp LSB = 1C
 x axis MSB = 1D, x axis LSB = 1E
 y axis MSB = 1F, y axis LSB = 20
 z axis MSB = 21, z axis LSB = 22
 *************************************/
 int regAddress = 0x1B;
 int temp, x, y, z;
 byte buff[G_TO_READ];
 readFrom(GYRO, regAddress, G_TO_READ, buff); //read the gyro data from the ITG3200
 result[0] = ((buff[2] << 8) | buff[3]) + g_offx;
 result[1] = ((buff[4] << 8) | buff[5]) + g_offy;
 result[2] = ((buff[6] << 8) | buff[7]) + g_offz;
 result[3] = (buff[0] << 8) | buff[1]; // temperature
 }
 //
void setup()
{
 Serial.begin(9600);
 Wire.begin();
 initGyro();
 // Настроим все необходимые пины на выход
  pinMode(STEP, OUTPUT);
  pinMode(DIR, OUTPUT);
  pinMode(EN, OUTPUT);
  oldvl=0;
}
//
void loop()
{
 byte addr;
 int gyro[4];
 int i;
 getGyroscopeData(gyro);
 hx = gyro[0] / 14.375;
 hy = gyro[1] / 14.375;
 hz = gyro[2] / 14.375;
 turetemp = 35+ ((double) (gyro[3] + 13200)) / 280; // temperature
 Serial.print(" X=");
 Serial.print(hx);
 Serial.print(" Y=");
 Serial.print(hy);
 Serial.print(" Z=");
 Serial.print(hz);
 Serial.print(" F=");
 Serial.print(turetemp);
 Serial.print((char)223);
 Serial.println("C");
 delay(50); 
 
 if(oldvl<hx){
   tmp=abs(hx)-abs(oldvl);
   oldvl=hx;
   Serial.print(" Vlevo shagov = ");
   Serial.print(tmp);
   digitalWrite(EN, HIGH);
   digitalWrite(DIR, HIGH);
   if(tmp<=0)tmp=1;
   for(i=0;i<tmp;i++){
      digitalWrite(STEP, HIGH);
      delay(DELAY_TIME);
      digitalWrite(STEP, LOW);
      delay(DELAY_TIME);
   }
   digitalWrite(EN, LOW);
   Serial.print(" STOIM! ");

 }
 
 if(oldvl>hx){
   tmp=abs(oldvl)-abs(hx);
   oldvl=hx;
   Serial.print(" Vpravo shagov = ");
   Serial.print(tmp);
   digitalWrite(EN, HIGH);
   digitalWrite(DIR, LOW);
   if(tmp<=0)tmp=1;
   for(i=0;i<tmp;i++){
      digitalWrite(STEP, HIGH);
      delay(DELAY_TIME);
      digitalWrite(STEP, LOW);
      delay(DELAY_TIME);
   }
   digitalWrite(EN, LOW);
   Serial.print(" STOIM! ");
   
 }
 
 
}
//---------------- Functions
//Writes val to address register on ACC
void writeTo(int DEVICE, byte address, byte val) {
  Wire.beginTransmission(DEVICE); //start transmission to ACC 
  Wire.write(address);        // send register address
  Wire.write(val);        // send value to write
  Wire.endTransmission(); //end transmission
}
//reads num bytes starting from address register on ACC in to buff array
 void readFrom(int DEVICE, byte address, int num, byte buff[]) {
 Wire.beginTransmission(DEVICE); //start transmission to ACC 
 Wire.write(address);        //sends address to read from
 Wire.endTransmission(); //end transmission
 
 Wire.beginTransmission(DEVICE); //start transmission to ACC
 Wire.requestFrom(DEVICE, num);    // request 6 bytes from ACC
 
 int i = 0;
 while(Wire.available())    //ACC may send less than requested (abnormal)
 { 
   buff[i] = Wire.read(); // receive a byte
   i++;
 }
 Wire.endTransmission(); //end transmission
}