File content as of revision 0:b0eda0d0afb1:

#include <mbed.h>
#include <PCA9685.h>
#include "INA219.hpp"            

PCA9685 pwm;
Serial ma(USBTX,USBRX);
    
//サーボモータ
//135°のとき700μs
//-135°のとき2300μs
#define SERVOMIN 700       
#define SERVOMAX 2300
Serial pcd(USBTX,USBRX);
// Get this to work with the INA219 Breakout from Adafruit
//INA219 ina219(p28, p27, 0x40, 400000, RES_10BITS);//1
//INA219 ina219(p28, p27, 0x41, 400000, RES_10BITS);//2
//INA219 ina219(p28, p27, 0x42, 400000, RES_10BITS);//3
//INA219 ina219(p28, p27, 0x43, 400000, RES_10BITS);//4
//INA219 ina219(p28, p27, 0x44, 400000, RES_10BITS);//5
//INA219 ina219(p28, p27, 0x45, 400000, RES_10BITS);//6
//INA219 ina219(p28, p27, 0x46, 400000, RES_10BITS);//7
//INA219 ina219(p28, p27, 0x47, 400000, RES_10BITS);//8
//INA219 ina219(p28, p27, 0x48, 400000, RES_10BITS);//9
//INA219 ina219(p28, p27, 0x49, 400000, RES_10BITS);//10
//INA219 ina219(p28, p27, 0x4a, 400000, RES_10BITS);//11
//INA219 ina219(p28, p27, 0x4b, 400000, RES_10BITS);//12
INA219 ina219(p28, p27, 0x4c, 400000, RES_10BITS);//13
//INA219 ina219(p28, p27, 0x4d, 400000, RES_10BITS);//14
//INA219 ina219(p28, p27, 0x4e, 400000, RES_10BITS);//15
//INA219 ina219(p28, p27, 0x4f, 400000, RES_10BITS);//16


Ticker measure;
float refresh_rate = 10;

void show_current()
{
    double current_ma = ina219.read_current_mA();
    ma.printf("%lf\r\n", current_ma);

}
      
void setup() { 
 pwm.begin();
                
 pwm.setPWMFreq(200);                
}


void servo_write7(int ch, double ang){ 
  ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
  //ma.printf("ang=%5.0lf \r\n ",ang) ;                                               //初期状態を設定するときこの値を参考に設定したためそのまま利用　　 
  pwm.setPWM(ch, 0, ang);
}

void loop() {
 


  //初期状態
  servo_write7(0, 6400);
  servo_write7(1, 6400);
  servo_write7(2, 6400);
  servo_write7(3, 6400);
  servo_write7(4, 6400);
  servo_write7(5, 6400);
  servo_write7(6, 6400);
  servo_write7(7, 6400);
  servo_write7(8, 6400);
  servo_write7(9, 6400);
  servo_write7(10, 6400);
  servo_write7(11, 6400);
  servo_write7(12, 6400);
  servo_write7(13, 6400);
  servo_write7(14, 6400);
  servo_write7(15, 6400);
  
  //servo_write7(1, 6200);
  //servo_write7(2, 7700); 
  //servo_write7(3, 7350);
  //servo_write7(4, 6360);
  //servo_write7(5, 5300);
  //servo_write7(6, 7600);
  //servo_write7(7, 6900);
  //servo_write7(8, 8100);
  //servo_write7(9, 6400);
  //servo_write7(10, 8700);
  //servo_write7(11, 5250);
  
  wait(1); 
  
  servo_write7(0, 8400);
  servo_write7(1, 8400);
  servo_write7(2, 8400);
  servo_write7(3, 8400);
  servo_write7(4, 8400);
  servo_write7(5, 8400);
  servo_write7(6, 8400);
  servo_write7(7, 8400);
  servo_write7(8, 8400);
  servo_write7(9, 8400);
  servo_write7(10, 8400);
  servo_write7(11, 8400);
  servo_write7(12, 8400);
  servo_write7(13, 8400);
  servo_write7(14, 8400);
  servo_write7(15, 8400);
  //servo_write7(1, 6200);
  //servo_write7(2, 7700); 
  //servo_write7(3, 7350);
  //servo_write7(4, 6360);
  //servo_write7(5, 5300);
  //servo_write7(6, 7600);
  //servo_write7(7, 6900);
  //servo_write7(8, 8100);
  //servo_write7(9, 6400);
  //servo_write7(10, 8700);
  //servo_write7(11, 5250);
  
  wait(1);

      
}

int main(){
    setup();
     //ma.baud(921600);
    for(int i=0;i<3;i++)
    {loop();}
    float refresh_interval = 1/refresh_rate;
    measure.attach(&show_current, refresh_interval);
    while(1){
        loop();
        }
        }