Diff: main.cpp

Revision:

0:b0eda0d0afb1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Dec 17 05:32:27 2021 +0000
@@ -0,0 +1,136 @@
+
+#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();
+        }
+        }
\ No newline at end of file