Diff: main_processing/strategy_parts/output.cpp

Revision:

28:8ac6c3c1e643

Parent:

21:d69a8f3c76e1

Child:

34:69bdf29a4442

--- a/main_processing/strategy_parts/output.cpp	Sun Jan 31 15:46:19 2016 +0000
+++ b/main_processing/strategy_parts/output.cpp	Mon Feb 01 02:05:44 2016 +0000
@@ -1,2 +1,80 @@
 #include "mbed.h"
 #include "extern.h"
+
+//pid&cmps
+void PidUpdate(void)
+{   
+    pid.setSetPoint(REFERENCE + data.FrontDeg); 
+    data.cmps = hmc.sample()/10.0;
+    data.InputPID = fmod((data.cmps+data.CmpsDiff+720.0), 360.0);//0<data.cmps<359.0
+
+    pid.setProcessValue(data.InputPID);
+    data.OutputPID = -pid.compute();
+}
+void ValidPidUpdate(void){
+    if(data.PidFlag==0){
+        data.PidFlag=1;
+    }
+}
+//motor
+
+void ValidTx_motor(void){
+    if(data.MotorFlag==0){
+        data.MotorFlag=1;
+    }
+}
+void tx_motor(){//モーターへの送信
+    array2(-speed[1],-speed[0],speed[2],speed[3]);
+    motor.printf("%s",StringFIN.c_str());
+}
+void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
+    uint8_t i;
+    double pwm[4] = {0};
+    
+    pwm[0] = (double)((vx) + vs);
+    pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
+    pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
+    pwm[3] = 0;
+
+    for(i = 0; i < 4; i++){
+        if(pwm[i] > 100){
+            pwm[i] = 100;
+        } else if(pwm[i] < -100){
+            pwm[i] = -100;
+        }
+        speed[i] = pwm[i];
+    }
+    
+    data.motorlog[X_AXIS] += vx;
+    data.motorlog[Y_AXIS] += vy;
+}
+void move_rectan(int vx, int vy, int vs){//三輪オムニの移動（直交座標指定）
+    move(vx, vy, vs);
+}
+void move_polar(int degree, int power, int vs){//三輪オムニの移動（極座標指定）
+    int vx, vy, deg;
+    deg = (degree+5)/10;
+    vx = power*sin(DEG2RAD(deg));
+    vy = power*cos(DEG2RAD(deg));
+    move(vx, vy, vs);
+}
+void move_mouse(int32_t point[2], int vs){//三輪オムニの移動（マウスでの直交座標指定）
+    int vx,vy;
+    vx = (point[0] - data.mouse[0])/10000;
+    vy = (point[1] - data.mouse[1])/10000;
+    move(vx, vy, vs);
+}
+//solenoid
+void AvailableSolenoid(void){
+    if(data.KickFlag==0){
+        data.KickFlag = 1;
+    }
+}
+void DriveSolenoid(void){
+    data.KickFlag = 0;
+    kicker=1;
+    Solenoid_timeout.attach(&SolenoidSignal, 0.005);
+}
+void SolenoidSignal(void){
+    kicker=0;
+}
\ No newline at end of file