Diff: main_processing/strategy_parts/output.cpp

Revision:

0:ea35c18c85fc

Child:

2:635947de1583

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_processing/strategy_parts/output.cpp	Sat Feb 27 09:14:37 2016 +0000
@@ -0,0 +1,71 @@
+#include "mbed.h"
+#include "extern.h"
+
+//pid&cmps
+void PidUpdate(void)
+{   
+    pid.setSetPoint(REFERENCE + cmps_set.FrontDeg); 
+    cmps_set.cmps = hmc.sample()/10.0;
+    cmps_set.InputPID = fmod((cmps_set.cmps+cmps_set.CmpsDiff+720.0), 360.0);//0<cmps_set.cmps<359.0
+
+    pid.setProcessValue(cmps_set.InputPID);
+    cmps_set.OutputPID = -pid.compute();
+}
+void ValidPidUpdate(void){
+    if(sys.PidFlag==0){
+        sys.PidFlag=1;
+    }
+}
+//motor
+
+void ValidTx_motor(void){
+    if(sys.MotorFlag==0){
+        sys.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];
+    }
+}
+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);
+}
+//solenoid
+void AvailableSolenoid(void){
+    if(sys.KickFlag==0){
+        sys.KickFlag = 1;
+    }
+}
+void DriveSolenoid(void){
+    sys.KickFlag = 0;
+    kicker=1;
+    Solenoid_timeout.attach(&SolenoidSignal, .5);
+}
+void SolenoidSignal(void){
+    kicker=0;
+}
\ No newline at end of file