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?
User | Revision | Line number | New 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 | } |