![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy_parts/output.cpp
- Revision:
- 28:8ac6c3c1e643
- Parent:
- 21:d69a8f3c76e1
- Child:
- 34:69bdf29a4442
diff -r 769cb5a7ea37 -r 8ac6c3c1e643 main_processing/strategy_parts/output.cpp --- 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