Diff: YKNCT_I2C.cpp

Revision:

15:23cf4a802b5b

Parent:

11:c7c764e21224

Child:

18:c631bf8fc741

--- a/YKNCT_I2C.cpp	Tue Apr 23 11:04:54 2019 +0000
+++ b/YKNCT_I2C.cpp	Mon Apr 29 16:26:19 2019 +0000
@@ -7,10 +7,7 @@
 
 void Y_I2C::Out_Set(OUT_I2C_Data_TypeDef *OUT_I2C_Data, uint8_t num, uint8_t value)
 {
-    if(value==1)
-        (OUT_I2C_Data+num) -> out_data |= 1;
-    else
-        (OUT_I2C_Data+num) -> out_data &= 0;
+    (OUT_I2C_Data+num) -> out_data =value;
 }
 
 void Y_I2C::Out(OUT_I2C_Data_TypeDef *OUT_I2C_Data, uint8_t MAX)
@@ -31,6 +28,32 @@
     }
 }
 
+void Y_I2C::Servo_Set(SERVO_I2C_Data_TypeDef *SERVO_I2C_Data, uint8_t num, uint16_t pulse)
+{
+    (SERVO_I2C_Data+num)->pulse=pulse;
+}
+
+//4biteづつ送信する
+void Y_I2C::Servo(SERVO_I2C_Data_TypeDef *SERVO_I2C_Data, uint8_t MAX)
+{
+    for(int i=0; i<(MAX+1)/2; i++) {
+        char data[4]= {0};
+        
+        //MAXが奇数で最後の時
+        if(i==MAX/2 && MAX%2==1) {
+            for(int j=0; j<2; j++)
+                //16bitのpulse1つを8bitづつに分けてdataに保存する
+                data[j]=(SERVO_I2C_Data+i*2+j/2)->pulse>>(1-j%2)*8;
+        } else {
+            for(int j=0; j<4; j++)
+                //16bitのpulse2つを8bitづつに分けてdataに保存する
+                data[j]=(SERVO_I2C_Data+i*2+j/2)->pulse>>(1-j%2)*8;
+        }
+        i2c.write((SERVO_I2C_Data+i)->DevAddress<<1, data, 4);
+    }
+}
+
+
 void Y_I2C::In(IN_I2C_Data_TypeDef *IN_I2C_Data, uint8_t num)
 {
     char data;