Revision 0:6319bf6272ae, committed 2018-10-28

Comitter:

Yamanobe

Date:

Sun Oct 28 08:19:29 2018 +0000

Commit message:

?????;

Changed in this revision

diff -r 000000000000 -r 6319bf6272ae Adafruit_MotorShield/Adafruit_MotorShield.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_MotorShield/Adafruit_MotorShield.cpp	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,409 @@
+/******************************************************************
+ This is the library for the Adafruit Motor Shield V2 for Arduino. 
+ It supports DC motors & Stepper motors with microstepping as well
+ as stacking-support. It is *not* compatible with the V1 library!
+
+ It will only work with https://www.adafruit.com/products/1483
+ 
+ Adafruit invests time and resources providing this open
+ source code, please support Adafruit and open-source hardware
+ by purchasing products from Adafruit!
+ 
+ Written by Limor Fried/Ladyada for Adafruit Industries.
+ BSD license, check license.txt for more information.
+ All text above must be included in any redistribution.
+ ******************************************************************/
+
+//#include "mbed.h"
+#include "mbed_m.h"
+#include "Adafruit_MotorShield.h"
+#include "Adafruit_PWMServoDriver.h"
+
+#if (MICROSTEPS == 8)
+uint8_t microstepcurve[] = {0, 50, 98, 142, 180, 212, 236, 250, 255};
+#elif (MICROSTEPS == 16)
+uint8_t microstepcurve[] = {0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255};
+#endif
+
+Adafruit_MotorShield::Adafruit_MotorShield(I2C *i2c, uint8_t addr) 
+{
+  _i2c = i2c;
+  _addr = addr;
+  _pwm = new Adafruit_PWMServoDriver(_i2c, _addr);
+}
+
+void Adafruit_MotorShield::begin(uint16_t freq) {
+  // init PWM w/_freq
+   _pwm->begin();
+  _freq = freq;
+  _pwm->setPWMFreq(_freq);  // This is the maximum PWM frequency
+  for (uint8_t i=0; i<16; i++) 
+    _pwm->setPWM(i, 0, 0);
+}
+
+void Adafruit_MotorShield::setPWM(uint8_t pin, uint16_t value) {
+  ///_pwm.dumy_read(); /// noda_test
+  if (value > 4095) {
+    _pwm->setPWM(pin, 4096, 0);
+  } else 
+    _pwm->setPWM(pin, 0, value);
+}
+void Adafruit_MotorShield::setPin(uint8_t pin, bool value) {
+  if (value == 0)
+    _pwm->setPWM(pin, 0, 0);
+  else
+    _pwm->setPWM(pin, 4096, 0);
+}
+
+Adafruit_DCMotor *Adafruit_MotorShield::getMotor(uint8_t num) {
+  if (num > 4) return NULL;
+
+  num--;
+
+  if (dcmotors[num].motornum == 0) {
+    // not init'd yet!
+    dcmotors[num].motornum = num;
+    dcmotors[num].MC = this;
+    uint8_t pwm, in1, in2;
+    if (num == 0) {
+      pwm = 8; in2 = 9; in1 = 10;
+    } else if (num == 1) {
+      pwm = 13; in2 = 12; in1 = 11;
+    } else if (num == 2) {
+      pwm = 2; in2 = 3; in1 = 4;
+    } else if (num == 3) {
+      pwm = 7; in2 = 6; in1 = 5;
+    }
+    dcmotors[num].PWMpin = pwm;
+    dcmotors[num].IN1pin = in1;
+    dcmotors[num].IN2pin = in2;
+  }
+  return &dcmotors[num];
+}
+
+
+Adafruit_StepperMotor *Adafruit_MotorShield::getStepper(uint16_t steps, uint8_t num) {
+  if (num > 2) return NULL;
+
+  num--;
+
+  if (steppers[num].steppernum == 0) {
+    // not init'd yet!
+    steppers[num].steppernum = num;
+    steppers[num].revsteps = steps;
+    steppers[num].MC = this;
+    uint8_t pwma, pwmb, ain1, ain2, bin1, bin2;
+    if (num == 0) {
+      pwma = 8; ain2 = 9; ain1 = 10;
+      pwmb = 13; bin2 = 12; bin1 = 11;
+    } else if (num == 1) {
+      pwma = 2; ain2 = 3; ain1 = 4;
+      pwmb = 7; bin2 = 6; bin1 = 5;
+    }
+    steppers[num].PWMApin = pwma;
+    steppers[num].PWMBpin = pwmb;
+    steppers[num].AIN1pin = ain1;
+    steppers[num].AIN2pin = ain2;
+    steppers[num].BIN1pin = bin1;
+    steppers[num].BIN2pin = bin2;
+  }
+  return &steppers[num];
+}
+
+
+/******************************************
+               MOTORS
+******************************************/
+
+Adafruit_DCMotor::Adafruit_DCMotor(void) {
+  MC = NULL;
+  motornum = 0;
+  PWMpin = IN1pin = IN2pin = 0;
+}
+
+void Adafruit_DCMotor::run(uint8_t cmd) {
+  switch (cmd) {
+  case FORWARD:
+    MC->setPin(IN2pin, 0);  // take low first to avoid 'break'
+    MC->setPin(IN1pin, 1);
+    break;
+  case BACKWARD:
+    MC->setPin(IN1pin, 0);  // take low first to avoid 'break'
+    MC->setPin(IN2pin, 1);
+    break;
+  case RELEASE:
+    MC->setPin(IN1pin, 0);
+    MC->setPin(IN2pin, 0);
+    break;
+  }
+}
+
+void Adafruit_DCMotor::setSpeed(uint16_t speed) {
+  MC->setPWM(PWMpin, speed*16);
+}
+
+/******************************************
+               STEPPERS
+******************************************/
+
+Adafruit_StepperMotor::Adafruit_StepperMotor(void) {
+  revsteps = steppernum = currentstep = 0;
+}
+/*
+
+uint16_t steps, Adafruit_MotorShield controller)  {
+
+  revsteps = steps;
+  steppernum = 1;
+  currentstep = 0;
+
+  if (steppernum == 1) {
+    latch_state &= ~_BV(MOTOR1_A) & ~_BV(MOTOR1_B) &
+      ~_BV(MOTOR2_A) & ~_BV(MOTOR2_B); // all motor pins to 0
+    
+    // enable both H bridges
+    pinMode(11, OUTPUT);
+    pinMode(3, OUTPUT);
+    digitalWrite(11, HIGH);
+    digitalWrite(3, HIGH);
+
+    // use PWM for microstepping support
+    MC->setPWM(1, 255);
+    MC->setPWM(2, 255);
+
+  } else if (steppernum == 2) {
+    latch_state &= ~_BV(MOTOR3_A) & ~_BV(MOTOR3_B) &
+      ~_BV(MOTOR4_A) & ~_BV(MOTOR4_B); // all motor pins to 0
+
+    // enable both H bridges
+    pinMode(5, OUTPUT);
+    pinMode(6, OUTPUT);
+    digitalWrite(5, HIGH);
+    digitalWrite(6, HIGH);
+
+    // use PWM for microstepping support
+    // use PWM for microstepping support
+    MC->setPWM(3, 255);
+    MC->setPWM(4, 255);
+  }
+}
+*/
+
+void Adafruit_StepperMotor::setSpeed(uint16_t rpm) {
+  //Serial.println("steps per rev: "); Serial.println(revsteps);
+  //Serial.println("RPM: "); Serial.println(rpm);
+
+  usperstep = 60000000 / ((uint32_t)revsteps * (uint32_t)rpm);
+  steppingcounter = 0;
+}
+
+void Adafruit_StepperMotor::release(void) {
+  MC->setPin(AIN1pin, 0);
+  MC->setPin(AIN2pin, 0);
+  MC->setPin(BIN1pin, 0);
+  MC->setPin(BIN2pin, 0);
+  MC->setPWM(PWMApin, 0);
+  MC->setPWM(PWMBpin, 0);
+}
+
+void Adafruit_StepperMotor::step(uint16_t steps, uint8_t dir,  uint8_t style) {
+  uint32_t uspers = usperstep;
+  uint8_t ret = 0;
+
+  if (style == INTERLEAVE) {
+    uspers /= 2;
+  }
+ else if (style == MICROSTEP) {
+    uspers /= MICROSTEPS;
+    steps *= MICROSTEPS;
+#ifdef MOTORDEBUG
+   // Serial.print("steps = "); Serial.println(steps, DEC);
+#endif
+  }
+
+  while (steps--) {
+    //Serial.println("step!"); Serial.println(uspers);
+    ret = onestep(dir, style);
+    //delay(uspers/1000); // in ms
+    wait_ms((float)(uspers/1000));
+    steppingcounter += (uspers % 1000);
+    if (steppingcounter >= 1000) {
+      //delay(1);
+      wait_ms(1.0);
+      steppingcounter -= 1000;
+    }
+  }
+  if (style == MICROSTEP) {
+    while ((ret != 0) && (ret != MICROSTEPS)) {
+      ret = onestep(dir, style);
+      //delay(uspers/1000); // in ms
+      wait_ms((float)(uspers/1000));
+      steppingcounter += (uspers % 1000);
+      if (steppingcounter >= 1000) {
+	//delay(1);
+         wait_ms(1.0);
+	steppingcounter -= 1000;
+      } 
+    }
+  }
+}
+
+uint8_t Adafruit_StepperMotor::onestep(uint8_t dir, uint8_t style) {
+  uint8_t a, b, c, d;
+  uint8_t ocrb, ocra;
+
+  ocra = ocrb = 255;
+
+
+  // next determine what sort of stepping procedure we're up to
+  if (style == SINGLE) {
+    if ((currentstep/(MICROSTEPS/2)) % 2) { // we're at an odd step, weird
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS/2;
+      }
+      else {
+	currentstep -= MICROSTEPS/2;
+      }
+    } else {           // go to the next even step
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS;
+      }
+      else {
+	currentstep -= MICROSTEPS;
+      }
+    }
+  } else if (style == DOUBLE) {
+    if (! (currentstep/(MICROSTEPS/2) % 2)) { // we're at an even step, weird
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS/2;
+      } else {
+	currentstep -= MICROSTEPS/2;
+      }
+    } else {           // go to the next odd step
+      if (dir == FORWARD) {
+	currentstep += MICROSTEPS;
+      } else {
+	currentstep -= MICROSTEPS;
+      }
+    }
+  } else if (style == INTERLEAVE) {
+    if (dir == FORWARD) {
+       currentstep += MICROSTEPS/2;
+    } else {
+       currentstep -= MICROSTEPS/2;
+    }
+  } 
+
+  if (style == MICROSTEP) {
+    if (dir == FORWARD) {
+      currentstep++;
+    } else {
+      // BACKWARDS
+      currentstep--;
+    }
+
+    currentstep += MICROSTEPS*4;
+    currentstep %= MICROSTEPS*4;
+
+    ocra = ocrb = 0;
+    if ( (currentstep >= 0) && (currentstep < MICROSTEPS)) {
+      ocra = microstepcurve[MICROSTEPS - currentstep];
+      ocrb = microstepcurve[currentstep];
+    } else if  ( (currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2)) {
+      ocra = microstepcurve[currentstep - MICROSTEPS];
+      ocrb = microstepcurve[MICROSTEPS*2 - currentstep];
+    } else if  ( (currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3)) {
+      ocra = microstepcurve[MICROSTEPS*3 - currentstep];
+      ocrb = microstepcurve[currentstep - MICROSTEPS*2];
+    } else if  ( (currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4)) {
+      ocra = microstepcurve[currentstep - MICROSTEPS*3];
+      ocrb = microstepcurve[MICROSTEPS*4 - currentstep];
+    }
+  }
+
+  currentstep += MICROSTEPS*4;
+  currentstep %= MICROSTEPS*4;
+
+#ifdef MOTORDEBUG
+  Serial.print("current step: "); Serial.println(currentstep, DEC);
+  Serial.print(" pwmA = "); Serial.print(ocra, DEC); 
+  Serial.print(" pwmB = "); Serial.println(ocrb, DEC); 
+#endif
+  MC->setPWM(PWMApin, ocra*16);
+  MC->setPWM(PWMBpin, ocrb*16);
+  
+
+  // release all
+  uint8_t latch_state = 0; // all motor pins to 0
+
+  //Serial.println(step, DEC);
+  if (style == MICROSTEP) {
+    if ((currentstep >= 0) && (currentstep < MICROSTEPS))
+      latch_state |= 0x03;
+    if ((currentstep >= MICROSTEPS) && (currentstep < MICROSTEPS*2))
+      latch_state |= 0x06;
+    if ((currentstep >= MICROSTEPS*2) && (currentstep < MICROSTEPS*3))
+      latch_state |= 0x0C;
+    if ((currentstep >= MICROSTEPS*3) && (currentstep < MICROSTEPS*4))
+      latch_state |= 0x09;
+  } else {
+    switch (currentstep/(MICROSTEPS/2)) {
+    case 0:
+      latch_state |= 0x1; // energize coil 1 only
+      break;
+    case 1:
+      latch_state |= 0x3; // energize coil 1+2
+      break;
+    case 2:
+      latch_state |= 0x2; // energize coil 2 only
+      break;
+    case 3:
+      latch_state |= 0x6; // energize coil 2+3
+      break;
+    case 4:
+      latch_state |= 0x4; // energize coil 3 only
+      break; 
+    case 5:
+      latch_state |= 0xC; // energize coil 3+4
+      break;
+    case 6:
+      latch_state |= 0x8; // energize coil 4 only
+      break;
+    case 7:
+      latch_state |= 0x9; // energize coil 1+4
+      break;
+    }
+  }
+#ifdef MOTORDEBUG
+  Serial.print("Latch: 0x"); Serial.println(latch_state, HEX);
+#endif
+
+  if (latch_state & 0x1) {
+   // Serial.println(AIN2pin);
+    MC->setPin(AIN2pin, 1);
+  } else {
+    MC->setPin(AIN2pin, 0);
+  }
+  if (latch_state & 0x2) {
+    MC->setPin(BIN1pin, 1);
+   // Serial.println(BIN1pin);
+  } else {
+    MC->setPin(BIN1pin, 0);
+  }
+  if (latch_state & 0x4) {
+    MC->setPin(AIN1pin, 1);
+   // Serial.println(AIN1pin);
+  } else {
+    MC->setPin(AIN1pin, 0);
+  }
+  if (latch_state & 0x8) {
+    MC->setPin(BIN2pin, 1);
+   // Serial.println(BIN2pin);
+  } else {
+    MC->setPin(BIN2pin, 0);
+  }
+
+  return currentstep;
+}
+
+

diff -r 000000000000 -r 6319bf6272ae Adafruit_MotorShield/Adafruit_MotorShield.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_MotorShield/Adafruit_MotorShield.h	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,104 @@
+/******************************************************************
+ This is the library for the Adafruit Motor Shield V2 for Arduino. 
+ It supports DC motors & Stepper motors with microstepping as well
+ as stacking-support. It is *not* compatible with the V1 library!
+
+ It will only work with https://www.adafruit.com/products/1483
+ 
+ Adafruit invests time and resources providing this open
+ source code, please support Adafruit and open-source hardware
+ by purchasing products from Adafruit!
+ 
+ Written by Limor Fried/Ladyada for Adafruit Industries.
+ BSD license, check license.txt for more information.
+ All text above must be included in any redistribution.
+ ******************************************************************/
+
+#ifndef _Adafruit_MotorShield_h_
+#define _Adafruit_MotorShield_h_
+
+//#include "mbed.h"
+#include "mbed_m.h"
+#include "Adafruit_PWMServoDriver.h"
+
+//#define MOTORDEBUG
+
+#define MICROSTEPS 16         // 8 or 16
+
+#define MOTOR1_A 2
+#define MOTOR1_B 3
+#define MOTOR2_A 1
+#define MOTOR2_B 4
+#define MOTOR4_A 0
+#define MOTOR4_B 6
+#define MOTOR3_A 5
+#define MOTOR3_B 7
+
+#define FORWARD 1
+#define BACKWARD 2
+#define BRAKE 3
+#define RELEASE 4
+
+#define SINGLE 1
+#define DOUBLE 2
+#define INTERLEAVE 3
+#define MICROSTEP 4
+
+class Adafruit_MotorShield;
+
+class Adafruit_DCMotor
+{
+ public:
+  Adafruit_DCMotor(void);
+  friend class Adafruit_MotorShield;
+  void run(uint8_t);
+  void setSpeed(uint16_t);
+  
+ private:
+  uint8_t PWMpin, IN1pin, IN2pin;
+  Adafruit_MotorShield *MC;
+  uint8_t motornum;
+};
+
+class Adafruit_StepperMotor {
+ public:
+  Adafruit_StepperMotor(void);
+  friend class Adafruit_MotorShield;
+
+  void step(uint16_t steps, uint8_t dir,  uint8_t style = SINGLE);
+  void setSpeed(uint16_t);
+  uint8_t onestep(uint8_t dir, uint8_t style);
+  void release(void);
+  uint32_t usperstep, steppingcounter;
+
+ private:
+  uint8_t PWMApin, AIN1pin, AIN2pin;
+  uint8_t PWMBpin, BIN1pin, BIN2pin;
+  uint16_t revsteps; // # steps per revolution
+  uint8_t currentstep;
+  Adafruit_MotorShield *MC;
+  uint8_t steppernum;
+};
+
+class Adafruit_MotorShield
+{
+  public:
+    Adafruit_MotorShield(I2C *i2c, uint8_t addr = 0x60);
+    friend class Adafruit_DCMotor;
+    void begin(uint16_t freq = 1600);
+
+    void setPWM(uint8_t pin, uint16_t val);
+    void setPin(uint8_t pin, bool val);
+    Adafruit_DCMotor *getMotor(uint8_t n);
+    Adafruit_StepperMotor *getStepper(uint16_t steps, uint8_t n);
+ private:
+    I2C *_i2c;
+    uint8_t _addr;
+    uint16_t _freq;
+    Adafruit_DCMotor dcmotors[4];
+    Adafruit_StepperMotor steppers[2];
+    Adafruit_PWMServoDriver* _pwm;
+};
+
+#endif
+

diff -r 000000000000 -r 6319bf6272ae Adafruit_PWMServoDriver/Adafruit_PWMServoDriver.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_PWMServoDriver/Adafruit_PWMServoDriver.cpp	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,125 @@
+/*************************************************** 
+  This is a library for our Adafruit 16-channel PWM & Servo driver
+
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/products/815
+
+  These displays use I2C to communicate, 2 pins are required to  
+  interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4
+
+  Adafruit invests time and resources providing this open source code, 
+  please support Adafruit and open-source hardware by purchasing 
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.  
+  BSD license, all text above must be included in any redistribution
+ ****************************************************/
+
+#include "Adafruit_PWMServoDriver.h"
+void dumy_handler(int events);
+//I2C i2c(I2C_SDA, I2C_SCL);
+
+
+
+Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(I2C *i2c, uint8_t addr)
+{
+  _i2c = i2c;
+  _i2caddr = addr;
+  _dumy_callback.attach(dumy_handler);
+}
+
+void Adafruit_PWMServoDriver::begin(void) 
+{
+    reset();
+}
+
+
+void Adafruit_PWMServoDriver::reset(void) 
+{
+ write8(PCA9685_MODE1, 0x0);
+}
+
+void Adafruit_PWMServoDriver::setPWMFreq(float freq) 
+{
+  //Serial.print("Attempting to set freq ");
+  //Serial.println(freq);
+  freq *= 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
+  float prescaleval = 25000000;
+  prescaleval /= 4096;
+  prescaleval /= freq;
+  prescaleval -= 1;
+  //Serial.print("Estimated pre-scale: "); Serial.println(prescaleval);
+  uint8_t prescale = (uint8_t)(floor((double)(prescaleval + 0.5)));
+  //Serial.print("Final pre-scale: "); Serial.println(prescale);  
+  
+  uint8_t oldmode = read8(PCA9685_MODE1);
+  uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
+  write8(PCA9685_MODE1, newmode); // go to sleep
+  write8(PCA9685_PRESCALE, prescale); // set the prescaler
+  write8(PCA9685_MODE1, oldmode);
+  //delay(5);
+  wait_ms((float)5.0);
+  write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
+                                          // This is why the beginTransmission below was not working.
+  //  Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX);
+}
+
+void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) 
+{
+char buf[5],rbuf[1];
+buf[0] = (char)(LED0_ON_L+4*num);
+buf[1] = (char)(on & 0x00ff);
+buf[2] = (char)((on>>8) & 0x00ff);
+buf[3] = (char) (off & 0x00ff);
+buf[4] = (char)((off>>8) & 0x00ff);
+_i2c->abort_transfer();
+
+if(_i2c->write((int)(_i2caddr<<1) , buf , 0, 0)==0){
+    //wait_us(100);
+    if(_i2c->write((int)(_i2caddr<<1) , buf , 5, 0)!=5) {
+        _i2c->reset();
+        //i2c.stop();
+        //i2c.abort_transfer();
+        }
+}
+else{_i2c->reset();}
+//i2c.transfer((int)(_i2caddr<<1), buf , 5, rbuf, 0, _dumy_callback);
+}
+
+uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) 
+{
+char buf[2];
+char ch = (char)addr;
+_i2c->write((int)(_i2caddr<<1) , &ch , 1, 0);
+_i2c->read((int)(_i2caddr<<1) , buf , 1 , 0);
+return (uint8_t)buf[0];
+}
+
+void Adafruit_PWMServoDriver::write8(uint8_t addr, uint8_t d) 
+{
+char buf[2],rbuf[1];
+buf[0] = (char)addr;
+buf[1] = (char)d;
+_i2c->abort_transfer();
+if(_i2c->write((int)(_i2caddr<<1) , buf , 0, 0)==0){
+    //wait_us(100);
+    if(_i2c->write((int)(_i2caddr<<1) , buf , 2, 0)!=2){ 
+    _i2c->reset();
+    //i2c.stop();
+    //i2c.abort_transfer();
+    }
+//i2c.transfer((int)(_i2caddr<<1), buf , 2, rbuf, 0, _dumy_callback);
+}
+else{ _i2c->reset();}
+}
+
+void Adafruit_PWMServoDriver::dumy_read(void)
+{
+ read8(0);
+ return;    
+}
+
+void dumy_handler(int events)
+{
+}
+

diff -r 000000000000 -r 6319bf6272ae Adafruit_PWMServoDriver/Adafruit_PWMServoDriver.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Adafruit_PWMServoDriver/Adafruit_PWMServoDriver.h	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,64 @@
+/*************************************************** 
+  This is a library for our Adafruit 16-channel PWM & Servo driver
+
+  Pick one up today in the adafruit shop!
+  ------> http://www.adafruit.com/products/815
+
+  These displays use I2C to communicate, 2 pins are required to  
+  interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4
+
+  Adafruit invests time and resources providing this open source code, 
+  please support Adafruit and open-source hardware by purchasing 
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.  
+  BSD license, all text above must be included in any redistribution
+ ****************************************************/
+
+#ifndef _ADAFRUIT_PWMServoDriver_H
+#define _ADAFRUIT_PWMServoDriver_H
+
+//#include "mbed.h"
+#include "mbed_m.h"
+
+#define PCA9685_SUBADR1 0x2
+#define PCA9685_SUBADR2 0x3
+#define PCA9685_SUBADR3 0x4
+
+#define PCA9685_MODE1 0x0
+#define PCA9685_PRESCALE 0xFE
+
+#define LED0_ON_L 0x6
+#define LED0_ON_H 0x7
+#define LED0_OFF_L 0x8
+#define LED0_OFF_H 0x9
+
+#define ALLLED_ON_L 0xFA
+#define ALLLED_ON_H 0xFB
+#define ALLLED_OFF_L 0xFC
+#define ALLLED_OFF_H 0xFD
+
+
+class Adafruit_PWMServoDriver {
+ public:
+  Adafruit_PWMServoDriver(I2C *i2c, uint8_t addr = 0x40);
+  //Adafruit_PWMServoDriver(uint8_t addr = 0x60);
+  void begin(void);
+  void reset(void);
+  void setPWMFreq(float freq);
+  void setPWM(uint8_t num, uint16_t on, uint16_t off);
+  void dumy_read(void);
+  
+ private:
+  I2C *_i2c;
+  uint8_t _i2caddr;
+ 
+  uint8_t read8(uint8_t addr);
+  void write8(uint8_t addr, uint8_t d);
+  event_callback_t _dumy_callback;
+  
+
+};
+
+#endif
+

diff -r 000000000000 -r 6319bf6272ae BOARDC_BME280.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BOARDC_BME280.lib	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/Yajirushi/code/BOARDC_BME280/#2baa0f77d4d2

diff -r 000000000000 -r 6319bf6272ae BOARDC_BNO055.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BOARDC_BNO055.lib	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/Yajirushi/code/BOARDC_BNO055/#3807ce385b2a

diff -r 000000000000 -r 6319bf6272ae I2C_Beta/I2C_Beta.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2C_Beta/I2C_Beta.cpp	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,171 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "I2C_Beta.h"
+
+#if DEVICE_I2C
+
+namespace mbed {
+
+I2C *I2C::_owner = NULL;
+SingletonPtr<PlatformMutex> I2C::_mutex;
+
+I2C::I2C(PinName sda, PinName scl) :
+#if DEVICE_I2C_ASYNCH
+                                     _irq(this), _usage(DMA_USAGE_NEVER),
+#endif
+                                      _i2c(), _hz(100000) {
+    // No lock needed in the constructor
+
+    // The init function also set the frequency to 100000
+    i2c_init(&_i2c, sda, scl);
+
+    // Used to avoid unnecessary frequency updates
+    _owner = this;
+}
+
+void I2C::frequency(int hz) {
+    lock();
+    _hz = hz;
+
+    // We want to update the frequency even if we are already the bus owners
+    i2c_frequency(&_i2c, _hz);
+
+    // Updating the frequency of the bus we become the owners of it
+    _owner = this;
+    unlock();
+}
+
+void I2C::aquire() {
+    lock();
+    if (_owner != this) {
+        i2c_frequency(&_i2c, _hz);
+        _owner = this;
+    }
+    unlock();
+}
+
+// write - Master Transmitter Mode
+int I2C::write(int address, const char* data, int length, bool repeated) {
+    lock();
+    aquire();
+
+    int stop = (repeated) ? 0 : 1;
+    int written = i2c_write(&_i2c, address, data, length, stop);
+
+    unlock();
+    //return length != written;
+    return written;
+}
+
+int I2C::write(int data) {
+    lock();
+    int ret = i2c_byte_write(&_i2c, data);
+    unlock();
+    return ret;
+}
+
+// read - Master Reciever Mode
+int I2C::read(int address, char* data, int length, bool repeated) {
+    lock();
+    aquire();
+
+    int stop = (repeated) ? 0 : 1;
+    int read = i2c_read(&_i2c, address, data, length, stop);
+
+    unlock();
+    return length != read;
+}
+
+int I2C::read(int ack) {
+    lock();
+    int ret;
+    if (ack) {
+        ret = i2c_byte_read(&_i2c, 0);
+    } else {
+        ret = i2c_byte_read(&_i2c, 1);
+    }
+    unlock();
+    return ret;
+}
+
+void I2C::start(void) {
+    lock();
+    i2c_start(&_i2c);
+    unlock();
+}
+
+void I2C::stop(void) {
+    lock();
+    i2c_stop(&_i2c);
+    unlock();
+}
+
+void I2C::reset(void) {
+    lock();
+    i2c_reset(&_i2c);
+    unlock();
+}
+
+void I2C::lock() {
+    _mutex->lock();
+}
+
+void I2C::unlock() {
+    _mutex->unlock();
+}
+
+#if DEVICE_I2C_ASYNCH
+
+int I2C::transfer(int address, const char *tx_buffer, int tx_length, char *rx_buffer, int rx_length, const event_callback_t& callback, int event, bool repeated)
+{
+    lock();
+    if (i2c_active(&_i2c)) {
+        unlock();
+        return -1; // transaction ongoing
+    }
+    aquire();
+
+    _callback = callback;
+    int stop = (repeated) ? 0 : 1;
+    _irq.callback(&I2C::irq_handler_asynch);
+    i2c_transfer_asynch(&_i2c, (void *)tx_buffer, tx_length, (void *)rx_buffer, rx_length, address, stop, _irq.entry(), event, _usage);
+    unlock();
+    return 0;
+}
+
+void I2C::abort_transfer(void)
+{
+    lock();
+    i2c_abort_asynch(&_i2c);
+    unlock();
+}
+
+void I2C::irq_handler_asynch(void)
+{
+    int event = i2c_irq_handler_asynch(&_i2c);
+    if (_callback && event) {
+        _callback.call(event);
+    }
+
+}
+
+
+#endif
+
+} // namespace mbed
+
+#endif
+

diff -r 000000000000 -r 6319bf6272ae I2C_Beta/I2C_Beta.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/I2C_Beta/I2C_Beta.h	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,201 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef MBED_I2C_BETA_H
+#define MBED_I2C_BETA_H
+
+#include "platform/platform.h"
+
+#if defined (DEVICE_I2C) || defined(DOXYGEN_ONLY)
+
+#include "hal/i2c_api.h"
+#include "platform/SingletonPtr.h"
+#include "platform/PlatformMutex.h"
+
+#if DEVICE_I2C_ASYNCH
+#include "platform/CThunk.h"
+#include "hal/dma_api.h"
+#include "platform/FunctionPointer.h"
+#endif
+
+namespace mbed {
+/** \addtogroup drivers */
+
+/** An I2C Master, used for communicating with I2C slave devices
+ *
+ * @note Synchronization level: Thread safe
+ *
+ * Example:
+ * @code
+ * // Read from I2C slave at address 0x62
+ *
+ * #include "mbed.h"
+ *
+ * I2C i2c(p28, p27);
+ *
+ * int main() {
+ *     int address = 0x62;
+ *     char data[2];
+ *     i2c.read(address, data, 2);
+ * }
+ * @endcode
+ * @ingroup drivers
+ */
+class I2C {
+
+public:
+    enum RxStatus {
+        NoData,
+        MasterGeneralCall,
+        MasterWrite,
+        MasterRead
+    };
+
+    enum Acknowledge {
+        NoACK = 0,
+        ACK   = 1
+    };
+
+    /** Create an I2C Master interface, connected to the specified pins
+     *
+     *  @param sda I2C data line pin
+     *  @param scl I2C clock line pin
+     */
+    I2C(PinName sda, PinName scl);
+
+    /** Set the frequency of the I2C interface
+     *
+     *  @param hz The bus frequency in hertz
+     */
+    void frequency(int hz);
+
+    /** Read from an I2C slave
+     *
+     * Performs a complete read transaction. The bottom bit of
+     * the address is forced to 1 to indicate a read.
+     *
+     *  @param address 8-bit I2C slave address [ addr | 1 ]
+     *  @param data Pointer to the byte-array to read data in to
+     *  @param length Number of bytes to read
+     *  @param repeated Repeated start, true - don't send stop at end
+     *
+     *  @returns
+     *       0 on success (ack),
+     *   non-0 on failure (nack)
+     */
+    int read(int address, char *data, int length, bool repeated = false);
+
+    /** Read a single byte from the I2C bus
+     *
+     *  @param ack indicates if the byte is to be acknowledged (1 = acknowledge)
+     *
+     *  @returns
+     *    the byte read
+     */
+    int read(int ack);
+
+    /** Write to an I2C slave
+     *
+     * Performs a complete write transaction. The bottom bit of
+     * the address is forced to 0 to indicate a write.
+     *
+     *  @param address 8-bit I2C slave address [ addr | 0 ]
+     *  @param data Pointer to the byte-array data to send
+     *  @param length Number of bytes to send
+     *  @param repeated Repeated start, true - do not send stop at end
+     *
+     *  @returns
+     *       0 on success (ack),
+     *   non-0 on failure (nack)
+     */
+    int write(int address, const char *data, int length, bool repeated = false);
+
+    /** Write single byte out on the I2C bus
+     *
+     *  @param data data to write out on bus
+     *
+     *  @returns
+     *    '0' - NAK was received
+     *    '1' - ACK was received,
+     *    '2' - timeout
+     */
+    int write(int data);
+
+    /** Creates a start condition on the I2C bus
+     */
+
+    void start(void);
+
+    /** Creates a stop condition on the I2C bus
+     */
+    void stop(void);
+    
+    /** reset I2C
+     */
+    void reset(void);
+
+    /** Acquire exclusive access to this I2C bus
+     */
+    virtual void lock(void);
+
+    /** Release exclusive access to this I2C bus
+     */
+    virtual void unlock(void);
+
+    virtual ~I2C() {
+        // Do nothing
+    }
+
+#if DEVICE_I2C_ASYNCH
+
+    /** Start non-blocking I2C transfer.
+     *
+     * @param address   8/10 bit I2c slave address
+     * @param tx_buffer The TX buffer with data to be transfered
+     * @param tx_length The length of TX buffer in bytes
+     * @param rx_buffer The RX buffer which is used for received data
+     * @param rx_length The length of RX buffer in bytes
+     * @param event     The logical OR of events to modify
+     * @param callback  The event callback function
+     * @param repeated Repeated start, true - do not send stop at end
+     * @return Zero if the transfer has started, or -1 if I2C peripheral is busy
+     */
+    int transfer(int address, const char *tx_buffer, int tx_length, char *rx_buffer, int rx_length, const event_callback_t& callback, int event = I2C_EVENT_TRANSFER_COMPLETE, bool repeated = false);
+
+    /** Abort the on-going I2C transfer
+     */
+    void abort_transfer();
+protected:
+    void irq_handler_asynch(void);
+    event_callback_t _callback;
+    CThunk<I2C> _irq;
+    DMAUsage _usage;
+#endif
+
+protected:
+    void aquire();
+
+    i2c_t _i2c;
+    static I2C  *_owner;
+    int         _hz;
+    static SingletonPtr<PlatformMutex> _mutex;
+};
+
+} // namespace mbed
+
+#endif
+
+#endif
+

diff -r 000000000000 -r 6319bf6272ae main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,141 @@
+#include "mbed_m.h"
+#include "Adafruit_PWMServoDriver.h"
+#include "Adafruit_MotorShield.h"
+#include "mbed.h"
+#include "BNO055.h"
+#include "BME280.h"
+
+//適宜定数を記述する
+#define del_t 0.0005 // 制御周期 0.005 ms
+#define Kp 5       //Pゲイン
+#define Ki 0.000     //Iゲイン
+#define Kd 0.00005  //Dゲイン
+
+//BNO055用の定義
+Serial pc(USBTX, USBRX);         //PCへのシリアル通信
+DigitalOut myled(LED1);          //mbed LED1
+I2C ifaceI2C(I2C_SDA, I2C_SCL);  //センサー用のI2C
+BOARDC_BNO055 sensor1(&ifaceI2C);//センサーで使うI2Cの定義
+//PID制御用
+Ticker flipper;
+Timer engi;
+I2C i2c(I2C_SDA, I2C_SCL);
+//モーターシールド用
+Adafruit_MotorShield AFMS(&i2c); 
+Adafruit_DCMotor *myMotor = AFMS.getMotor(2);
+
+int duty_value = 0;
+bool renew = false;
+void det_duty(int duty);
+float pitch;
+
+void det_duty(int duty)// duty: -255~255
+{
+    uint16_t abs_duty;    //モータの各動作の設定
+    if(duty < -255)    {
+        myMotor->run(BACKWARD);
+        myMotor->setSpeed(255);
+    }
+    else if(duty < 0)    {
+        abs_duty = (uint16_t)(-1* duty);
+        myMotor->run(BACKWARD);
+        myMotor->setSpeed(abs_duty);
+    }
+    else if(duty == 0)    {
+        myMotor->run(RELEASE);
+    }
+    else if(duty <= 255)    {
+        abs_duty = (uint16_t)(duty);
+        myMotor->run(FORWARD);
+        myMotor->setSpeed(abs_duty);
+    }
+    else    {
+        myMotor->run(FORWARD);
+        myMotor->setSpeed(255);
+    }
+}
+ 
+/// この箇所にPIDの制御を記述する．
+void flip()
+{
+    //適宜変数を追加する
+    int duty = 0;
+    float p,i,d;
+    float refe = 0.0;
+    float posi;
+    
+    static float diff[2] = {0,0};
+    static float inte = 0;
+    
+    posi = pitch;
+    
+    //
+    //PID
+    //
+    diff[0] = diff[1];
+    diff[1] = refe - posi;
+    inte += (diff[1] + diff[0])/2.0*del_t;
+    
+    p = Kp*diff[1];
+    i = Ki*inte;
+    d = Kd*(diff[1]-diff[0])/del_t;
+    duty = (int)(p+i+d);
+
+    //デューティ比を受け渡す
+    if(duty > 0 )
+    duty = 30 + duty; //myMotor->setSpeed(duty_value);
+    else if(duty < 0)
+    duty = -25 + duty;
+        
+    // 最大+-7.5Vを超えないようにデューティ比を調整
+    if(duty > 250 ) duty = 250;
+    else if(duty < -250) duty = -250;
+
+    duty_value = duty;
+    
+    myled != myled;
+    renew =true;
+}
+ 
+int main()
+{
+    pc.printf("Hello_mbed!\n");    //確認用
+
+    //I2Cインターフェースで２つのセンサーを使用する(200KHz)
+    ifaceI2C.frequency(200000);
+    sensor1.initialize(false);
+    engi.start();
+
+
+    //各センサーの値を格納するための変数宣言
+    short dataBox[12];
+    float scEUL;
+    scEUL = sensor1.getEulerScale();
+ 
+    pc.printf("Check!\n");          //確認用
+
+    while(1)
+    {
+        //配列dataBoxに、9軸の値とオイラー角(pitch)を格納
+        //倍率をかけてRaw値を実際の値に変換
+        sensor1.get9AxisAndEUL(dataBox);
+        pitch = (float)dataBox[11] * scEUL;
+        float time = engi.read();
+
+
+        //角度とデューティ比の表示
+        pc.printf("%05.4f,%05.4d,%4.1f\n", pitch,duty_value,time);
+
+        AFMS.begin();
+        myMotor->run(FORWARD);
+        myMotor->run(RELEASE);
+ 
+        flipper.attach(&flip, del_t);
+ 
+        if(renew)
+        {
+            det_duty(-1.0 * duty_value);    //打ち消す方向に変換
+            renew = false;
+        }
+    }
+}
\ No newline at end of file

diff -r 000000000000 -r 6319bf6272ae mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/7130f322cb7e
\ No newline at end of file

diff -r 000000000000 -r 6319bf6272ae mbed_m.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed_m.h	Sun Oct 28 08:19:29 2018 +0000
@@ -0,0 +1,119 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2006-2013 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef MBED_H
+#define MBED_H
+
+#define MBED_LIBRARY_VERSION 145
+
+#if MBED_CONF_RTOS_PRESENT
+// RTOS present, this is valid only for mbed OS 5
+#define MBED_MAJOR_VERSION 5
+#define MBED_MINOR_VERSION 5
+#define MBED_PATCH_VERSION 1
+
+#else
+// mbed 2
+#define MBED_MAJOR_VERSION 2
+#define MBED_MINOR_VERSION 0
+#define MBED_PATCH_VERSION MBED_LIBRARY_VERSION
+#endif
+
+#define MBED_ENCODE_VERSION(major, minor, patch) ((major)*10000 + (minor)*100 + (patch))
+#define MBED_VERSION MBED_ENCODE_VERSION(MBED_MAJOR_VERSION, MBED_MINOR_VERSION, MBED_PATCH_VERSION)
+
+#if MBED_CONF_RTOS_PRESENT
+#include "rtos/rtos.h"
+#endif
+
+#if MBED_CONF_NSAPI_PRESENT
+#include "netsocket/nsapi.h"
+#include "netsocket/nsapi_ppp.h"
+#endif
+
+#if MBED_CONF_EVENTS_PRESENT
+#include "events/mbed_events.h"
+#endif
+
+#if MBED_CONF_FILESYSTEM_PRESENT
+#include "filesystem/mbed_filesystem.h"
+#endif
+
+#include "platform/mbed_toolchain.h"
+#include "platform/platform.h"
+#include "platform/mbed_application.h"
+
+// Useful C libraries
+#include <math.h>
+#include <time.h>
+
+// mbed Debug libraries
+#include "platform/mbed_error.h"
+#include "platform/mbed_interface.h"
+#include "platform/mbed_assert.h"
+#include "platform/mbed_debug.h"
+
+// mbed Peripheral components
+#include "drivers/DigitalIn.h"
+#include "drivers/DigitalOut.h"
+#include "drivers/DigitalInOut.h"
+#include "drivers/BusIn.h"
+#include "drivers/BusOut.h"
+#include "drivers/BusInOut.h"
+#include "drivers/PortIn.h"
+#include "drivers/PortInOut.h"
+#include "drivers/PortOut.h"
+#include "drivers/AnalogIn.h"
+#include "drivers/AnalogOut.h"
+#include "drivers/PwmOut.h"
+#include "drivers/Serial.h"
+#include "drivers/SPI.h"
+#include "drivers/SPISlave.h"
+//#include "drivers/I2C.h"
+#include "I2C_Beta.h"
+#include "drivers/I2CSlave.h"
+#include "drivers/Ethernet.h"
+#include "drivers/CAN.h"
+#include "drivers/RawSerial.h"
+#include "drivers/UARTSerial.h"
+#include "drivers/FlashIAP.h"
+
+// mbed Internal components
+#include "drivers/Timer.h"
+#include "drivers/Ticker.h"
+#include "drivers/Timeout.h"
+#include "drivers/LowPowerTimeout.h"
+#include "drivers/LowPowerTicker.h"
+#include "drivers/LowPowerTimer.h"
+#include "platform/LocalFileSystem.h"
+#include "drivers/InterruptIn.h"
+#include "platform/mbed_wait_api.h"
+#include "hal/sleep_api.h"
+#include "platform/mbed_sleep.h"
+#include "platform/mbed_rtc_time.h"
+#include "platform/mbed_poll.h"
+#include "platform/ATCmdParser.h"
+#include "platform/FileSystemHandle.h"
+#include "platform/FileHandle.h"
+#include "platform/DirHandle.h"
+
+// mbed Non-hardware components
+#include "platform/Callback.h"
+#include "platform/FunctionPointer.h"
+
+using namespace mbed;
+using namespace std;
+
+#endif
\ No newline at end of file