output.cpp

00001 #include "mbed.h"
00002 #include "extern.h"
00003 
00004 //pid&cmps
00005 void PidUpdate(void)
00006 {   
00007     pid.setSetPoint(REFERENCE + data.FrontDeg); 
00008     data.cmps = hmc.sample()/10.0;
00009     data.InputPID = fmod((data.cmps+data.CmpsDiff+720.0), 360.0);//0<data.cmps<359.0
00010 
00011     pid.setProcessValue(data.InputPID);
00012     data.OutputPID = -pid.compute();
00013 }
00014 void ValidPidUpdate(void){
00015     if(data.PidFlag==0){
00016         data.PidFlag=1;
00017     }
00018 }
00019 //motor
00020 
00021 void ValidTx_motor(void){
00022     if(data.MotorFlag==0){
00023         data.MotorFlag=1;
00024     }
00025 }
00026 void tx_motor(){//モーターへの送信
00027     array2(speed[1],-speed[0],-speed[2],speed[3]);//右後左無
00028     motor.printf("%s",StringFIN.c_str());
00029 }
00030 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
00031     uint8_t i;
00032     double pwm[4] = {0};
00033     
00034     pwm[0] = (double)((vx) + vs);
00035     pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0)  * vy) + vs);
00036     pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
00037     pwm[3] = 0;
00038 
00039     for(i = 0; i < 4; i++){
00040         if(pwm[i] > 100){
00041             pwm[i] = 100;
00042         } else if(pwm[i] < -100){
00043             pwm[i] = -100;
00044         }
00045         speed[i] = pwm[i];
00046     }
00047     
00048     data.motorlog[X_AXIS] += vx;
00049     data.motorlog[Y_AXIS] += vy;
00050 }
00051 void move_rectan(int vx, int vy, int vs){//三輪オムニの移動（直交座標指定）
00052     move(vx, vy, vs);
00053 }
00054 void move_polar(int degree, int power, int vs){//三輪オムニの移動（極座標指定）
00055     int vx, vy, deg;
00056     deg = (degree+5)/10;
00057     vx = power*sin(DEG2RAD(deg));
00058     vy = power*cos(DEG2RAD(deg));
00059     move(vx, vy, vs);
00060 }
00061 void move_mouse(int32_t point[2], int vs){//三輪オムニの移動（マウスでの直交座標指定）
00062     int vx,vy;
00063     vx = (point[0] - data.mouse[0])/10000;
00064     vy = (point[1] - data.mouse[1])/10000;
00065     move(vx, vy, vs);
00066 }
00067 //solenoid
00068 void AvailableSolenoid(void){
00069     if(data.KickFlag==0){
00070         data.KickFlag = 1;
00071     }
00072 }
00073 void DriveSolenoid(void){
00074     data.KickFlag = 0;
00075     kicker=1;
00076     Solenoid_timeout.attach(&SolenoidSignal, .5);
00077 }
00078 void SolenoidSignal(void){
00079     kicker=0;
00080 }