Kotaro Irisawa / Mbed 2 deprecated INA_servo_test

2021.12.17

Dependencies:   mbed pca9685__12_17 INA219

main.cpp@0:b0eda0d0afb1, 2021-12-17 (annotated)

Committer:
Kotttaro
Date:
Fri Dec 17 05:32:27 2021 +0000
Revision:
0:b0eda0d0afb1
2021.12.17

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kotttaro 0:b0eda0d0afb1 1
Kotttaro 0:b0eda0d0afb1 2 #include <mbed.h>
Kotttaro 0:b0eda0d0afb1 3 #include <PCA9685.h>
Kotttaro 0:b0eda0d0afb1 4 #include "INA219.hpp"
Kotttaro 0:b0eda0d0afb1 5
Kotttaro 0:b0eda0d0afb1 6 PCA9685 pwm;
Kotttaro 0:b0eda0d0afb1 7 Serial ma(USBTX,USBRX);
Kotttaro 0:b0eda0d0afb1 8
Kotttaro 0:b0eda0d0afb1 9 //サーボモータ
Kotttaro 0:b0eda0d0afb1 10 //135°のとき700μs
Kotttaro 0:b0eda0d0afb1 11 //-135°のとき2300μs
Kotttaro 0:b0eda0d0afb1 12 #define SERVOMIN 700
Kotttaro 0:b0eda0d0afb1 13 #define SERVOMAX 2300
Kotttaro 0:b0eda0d0afb1 14 Serial pcd(USBTX,USBRX);
Kotttaro 0:b0eda0d0afb1 15 // Get this to work with the INA219 Breakout from Adafruit
Kotttaro 0:b0eda0d0afb1 16 //INA219 ina219(p28, p27, 0x40, 400000, RES_10BITS);//1
Kotttaro 0:b0eda0d0afb1 17 //INA219 ina219(p28, p27, 0x41, 400000, RES_10BITS);//2
Kotttaro 0:b0eda0d0afb1 18 //INA219 ina219(p28, p27, 0x42, 400000, RES_10BITS);//3
Kotttaro 0:b0eda0d0afb1 19 //INA219 ina219(p28, p27, 0x43, 400000, RES_10BITS);//4
Kotttaro 0:b0eda0d0afb1 20 //INA219 ina219(p28, p27, 0x44, 400000, RES_10BITS);//5
Kotttaro 0:b0eda0d0afb1 21 //INA219 ina219(p28, p27, 0x45, 400000, RES_10BITS);//6
Kotttaro 0:b0eda0d0afb1 22 //INA219 ina219(p28, p27, 0x46, 400000, RES_10BITS);//7
Kotttaro 0:b0eda0d0afb1 23 //INA219 ina219(p28, p27, 0x47, 400000, RES_10BITS);//8
Kotttaro 0:b0eda0d0afb1 24 //INA219 ina219(p28, p27, 0x48, 400000, RES_10BITS);//9
Kotttaro 0:b0eda0d0afb1 25 //INA219 ina219(p28, p27, 0x49, 400000, RES_10BITS);//10
Kotttaro 0:b0eda0d0afb1 26 //INA219 ina219(p28, p27, 0x4a, 400000, RES_10BITS);//11
Kotttaro 0:b0eda0d0afb1 27 //INA219 ina219(p28, p27, 0x4b, 400000, RES_10BITS);//12
Kotttaro 0:b0eda0d0afb1 28 INA219 ina219(p28, p27, 0x4c, 400000, RES_10BITS);//13
Kotttaro 0:b0eda0d0afb1 29 //INA219 ina219(p28, p27, 0x4d, 400000, RES_10BITS);//14
Kotttaro 0:b0eda0d0afb1 30 //INA219 ina219(p28, p27, 0x4e, 400000, RES_10BITS);//15
Kotttaro 0:b0eda0d0afb1 31 //INA219 ina219(p28, p27, 0x4f, 400000, RES_10BITS);//16
Kotttaro 0:b0eda0d0afb1 32
Kotttaro 0:b0eda0d0afb1 33
Kotttaro 0:b0eda0d0afb1 34 Ticker measure;
Kotttaro 0:b0eda0d0afb1 35 float refresh_rate = 10;
Kotttaro 0:b0eda0d0afb1 36
Kotttaro 0:b0eda0d0afb1 37 void show_current()
Kotttaro 0:b0eda0d0afb1 38 {
Kotttaro 0:b0eda0d0afb1 39 double current_ma = ina219.read_current_mA();
Kotttaro 0:b0eda0d0afb1 40 ma.printf("%lf\r\n", current_ma);
Kotttaro 0:b0eda0d0afb1 41
Kotttaro 0:b0eda0d0afb1 42 }
Kotttaro 0:b0eda0d0afb1 43
Kotttaro 0:b0eda0d0afb1 44 void setup() {
Kotttaro 0:b0eda0d0afb1 45 pwm.begin();
Kotttaro 0:b0eda0d0afb1 46
Kotttaro 0:b0eda0d0afb1 47 pwm.setPWMFreq(200);
Kotttaro 0:b0eda0d0afb1 48 }
Kotttaro 0:b0eda0d0afb1 49
Kotttaro 0:b0eda0d0afb1 50
Kotttaro 0:b0eda0d0afb1 51 void servo_write7(int ch, double ang){
Kotttaro 0:b0eda0d0afb1 52 ang = ((ang-3500)/8000)*1600+700;//サーボモータ内部エンコーダは8000段階
Kotttaro 0:b0eda0d0afb1 53 //ma.printf("ang=%5.0lf \r\n ",ang) ; //初期状態を設定するときこの値を参考に設定したためそのまま利用　　
Kotttaro 0:b0eda0d0afb1 54 pwm.setPWM(ch, 0, ang);
Kotttaro 0:b0eda0d0afb1 55 }
Kotttaro 0:b0eda0d0afb1 56
Kotttaro 0:b0eda0d0afb1 57 void loop() {
Kotttaro 0:b0eda0d0afb1 58
Kotttaro 0:b0eda0d0afb1 59
Kotttaro 0:b0eda0d0afb1 60
Kotttaro 0:b0eda0d0afb1 61 //初期状態
Kotttaro 0:b0eda0d0afb1 62 servo_write7(0, 6400);
Kotttaro 0:b0eda0d0afb1 63 servo_write7(1, 6400);
Kotttaro 0:b0eda0d0afb1 64 servo_write7(2, 6400);
Kotttaro 0:b0eda0d0afb1 65 servo_write7(3, 6400);
Kotttaro 0:b0eda0d0afb1 66 servo_write7(4, 6400);
Kotttaro 0:b0eda0d0afb1 67 servo_write7(5, 6400);
Kotttaro 0:b0eda0d0afb1 68 servo_write7(6, 6400);
Kotttaro 0:b0eda0d0afb1 69 servo_write7(7, 6400);
Kotttaro 0:b0eda0d0afb1 70 servo_write7(8, 6400);
Kotttaro 0:b0eda0d0afb1 71 servo_write7(9, 6400);
Kotttaro 0:b0eda0d0afb1 72 servo_write7(10, 6400);
Kotttaro 0:b0eda0d0afb1 73 servo_write7(11, 6400);
Kotttaro 0:b0eda0d0afb1 74 servo_write7(12, 6400);
Kotttaro 0:b0eda0d0afb1 75 servo_write7(13, 6400);
Kotttaro 0:b0eda0d0afb1 76 servo_write7(14, 6400);
Kotttaro 0:b0eda0d0afb1 77 servo_write7(15, 6400);
Kotttaro 0:b0eda0d0afb1 78
Kotttaro 0:b0eda0d0afb1 79 //servo_write7(1, 6200);
Kotttaro 0:b0eda0d0afb1 80 //servo_write7(2, 7700);
Kotttaro 0:b0eda0d0afb1 81 //servo_write7(3, 7350);
Kotttaro 0:b0eda0d0afb1 82 //servo_write7(4, 6360);
Kotttaro 0:b0eda0d0afb1 83 //servo_write7(5, 5300);
Kotttaro 0:b0eda0d0afb1 84 //servo_write7(6, 7600);
Kotttaro 0:b0eda0d0afb1 85 //servo_write7(7, 6900);
Kotttaro 0:b0eda0d0afb1 86 //servo_write7(8, 8100);
Kotttaro 0:b0eda0d0afb1 87 //servo_write7(9, 6400);
Kotttaro 0:b0eda0d0afb1 88 //servo_write7(10, 8700);
Kotttaro 0:b0eda0d0afb1 89 //servo_write7(11, 5250);
Kotttaro 0:b0eda0d0afb1 90
Kotttaro 0:b0eda0d0afb1 91 wait(1);
Kotttaro 0:b0eda0d0afb1 92
Kotttaro 0:b0eda0d0afb1 93 servo_write7(0, 8400);
Kotttaro 0:b0eda0d0afb1 94 servo_write7(1, 8400);
Kotttaro 0:b0eda0d0afb1 95 servo_write7(2, 8400);
Kotttaro 0:b0eda0d0afb1 96 servo_write7(3, 8400);
Kotttaro 0:b0eda0d0afb1 97 servo_write7(4, 8400);
Kotttaro 0:b0eda0d0afb1 98 servo_write7(5, 8400);
Kotttaro 0:b0eda0d0afb1 99 servo_write7(6, 8400);
Kotttaro 0:b0eda0d0afb1 100 servo_write7(7, 8400);
Kotttaro 0:b0eda0d0afb1 101 servo_write7(8, 8400);
Kotttaro 0:b0eda0d0afb1 102 servo_write7(9, 8400);
Kotttaro 0:b0eda0d0afb1 103 servo_write7(10, 8400);
Kotttaro 0:b0eda0d0afb1 104 servo_write7(11, 8400);
Kotttaro 0:b0eda0d0afb1 105 servo_write7(12, 8400);
Kotttaro 0:b0eda0d0afb1 106 servo_write7(13, 8400);
Kotttaro 0:b0eda0d0afb1 107 servo_write7(14, 8400);
Kotttaro 0:b0eda0d0afb1 108 servo_write7(15, 8400);
Kotttaro 0:b0eda0d0afb1 109 //servo_write7(1, 6200);
Kotttaro 0:b0eda0d0afb1 110 //servo_write7(2, 7700);
Kotttaro 0:b0eda0d0afb1 111 //servo_write7(3, 7350);
Kotttaro 0:b0eda0d0afb1 112 //servo_write7(4, 6360);
Kotttaro 0:b0eda0d0afb1 113 //servo_write7(5, 5300);
Kotttaro 0:b0eda0d0afb1 114 //servo_write7(6, 7600);
Kotttaro 0:b0eda0d0afb1 115 //servo_write7(7, 6900);
Kotttaro 0:b0eda0d0afb1 116 //servo_write7(8, 8100);
Kotttaro 0:b0eda0d0afb1 117 //servo_write7(9, 6400);
Kotttaro 0:b0eda0d0afb1 118 //servo_write7(10, 8700);
Kotttaro 0:b0eda0d0afb1 119 //servo_write7(11, 5250);
Kotttaro 0:b0eda0d0afb1 120
Kotttaro 0:b0eda0d0afb1 121 wait(1);
Kotttaro 0:b0eda0d0afb1 122
Kotttaro 0:b0eda0d0afb1 123
Kotttaro 0:b0eda0d0afb1 124 }
Kotttaro 0:b0eda0d0afb1 125
Kotttaro 0:b0eda0d0afb1 126 int main(){
Kotttaro 0:b0eda0d0afb1 127 setup();
Kotttaro 0:b0eda0d0afb1 128 //ma.baud(921600);
Kotttaro 0:b0eda0d0afb1 129 for(int i=0;i<3;i++)
Kotttaro 0:b0eda0d0afb1 130 {loop();}
Kotttaro 0:b0eda0d0afb1 131 float refresh_interval = 1/refresh_rate;
Kotttaro 0:b0eda0d0afb1 132 measure.attach(&show_current, refresh_interval);
Kotttaro 0:b0eda0d0afb1 133 while(1){
Kotttaro 0:b0eda0d0afb1 134 loop();
Kotttaro 0:b0eda0d0afb1 135 }
Kotttaro 0:b0eda0d0afb1 136 }