CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy_parts/output.cpp@45:c23f25c00d0d, 2016-02-25 (annotated)

Committer:
lilac0112_1
Date:
Thu Feb 25 06:12:47 2016 +0000
Revision:
45:c23f25c00d0d
Parent:
34:69bdf29a4442 
Hokushinetsu(20:1)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 19:342da3a5a474 1 #include "mbed.h"
lilac0112_1 19:342da3a5a474 2 #include "extern.h"
lilac0112_1 28:8ac6c3c1e643 3
lilac0112_1 28:8ac6c3c1e643 4 //pid&cmps
lilac0112_1 28:8ac6c3c1e643 5 void PidUpdate(void)
lilac0112_1 28:8ac6c3c1e643 6 {
lilac0112_1 28:8ac6c3c1e643 7 pid.setSetPoint(REFERENCE + data.FrontDeg);
lilac0112_1 28:8ac6c3c1e643 8 data.cmps = hmc.sample()/10.0;
lilac0112_1 28:8ac6c3c1e643 9 data.InputPID = fmod((data.cmps+data.CmpsDiff+720.0), 360.0);//0<data.cmps<359.0
lilac0112_1 28:8ac6c3c1e643 10
lilac0112_1 28:8ac6c3c1e643 11 pid.setProcessValue(data.InputPID);
lilac0112_1 28:8ac6c3c1e643 12 data.OutputPID = -pid.compute();
lilac0112_1 28:8ac6c3c1e643 13 }
lilac0112_1 28:8ac6c3c1e643 14 void ValidPidUpdate(void){
lilac0112_1 28:8ac6c3c1e643 15 if(data.PidFlag==0){
lilac0112_1 28:8ac6c3c1e643 16 data.PidFlag=1;
lilac0112_1 28:8ac6c3c1e643 17 }
lilac0112_1 28:8ac6c3c1e643 18 }
lilac0112_1 28:8ac6c3c1e643 19 //motor
lilac0112_1 28:8ac6c3c1e643 20
lilac0112_1 28:8ac6c3c1e643 21 void ValidTx_motor(void){
lilac0112_1 28:8ac6c3c1e643 22 if(data.MotorFlag==0){
lilac0112_1 28:8ac6c3c1e643 23 data.MotorFlag=1;
lilac0112_1 28:8ac6c3c1e643 24 }
lilac0112_1 28:8ac6c3c1e643 25 }
lilac0112_1 28:8ac6c3c1e643 26 void tx_motor(){//モーターへの送信
lilac0112_1 45:c23f25c00d0d 27 array2(speed[1],-speed[0],-speed[2],speed[3]);//右後左無
lilac0112_1 28:8ac6c3c1e643 28 motor.printf("%s",StringFIN.c_str());
lilac0112_1 28:8ac6c3c1e643 29 }
lilac0112_1 28:8ac6c3c1e643 30 void move(int vx, int vy, int vs){//三輪オムニの移動（基本の形）
lilac0112_1 28:8ac6c3c1e643 31 uint8_t i;
lilac0112_1 28:8ac6c3c1e643 32 double pwm[4] = {0};
lilac0112_1 28:8ac6c3c1e643 33
lilac0112_1 28:8ac6c3c1e643 34 pwm[0] = (double)((vx) + vs);
lilac0112_1 28:8ac6c3c1e643 35 pwm[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 28:8ac6c3c1e643 36 pwm[2] = (double)((-0.5 * vx) + ((-sqrt(3.0) / 2.0) * vy) + vs);
lilac0112_1 28:8ac6c3c1e643 37 pwm[3] = 0;
lilac0112_1 28:8ac6c3c1e643 38
lilac0112_1 28:8ac6c3c1e643 39 for(i = 0; i < 4; i++){
lilac0112_1 28:8ac6c3c1e643 40 if(pwm[i] > 100){
lilac0112_1 28:8ac6c3c1e643 41 pwm[i] = 100;
lilac0112_1 28:8ac6c3c1e643 42 } else if(pwm[i] < -100){
lilac0112_1 28:8ac6c3c1e643 43 pwm[i] = -100;
lilac0112_1 28:8ac6c3c1e643 44 }
lilac0112_1 28:8ac6c3c1e643 45 speed[i] = pwm[i];
lilac0112_1 28:8ac6c3c1e643 46 }
lilac0112_1 28:8ac6c3c1e643 47
lilac0112_1 28:8ac6c3c1e643 48 data.motorlog[X_AXIS] += vx;
lilac0112_1 28:8ac6c3c1e643 49 data.motorlog[Y_AXIS] += vy;
lilac0112_1 28:8ac6c3c1e643 50 }
lilac0112_1 28:8ac6c3c1e643 51 void move_rectan(int vx, int vy, int vs){//三輪オムニの移動（直交座標指定）
lilac0112_1 28:8ac6c3c1e643 52 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 53 }
lilac0112_1 28:8ac6c3c1e643 54 void move_polar(int degree, int power, int vs){//三輪オムニの移動（極座標指定）
lilac0112_1 28:8ac6c3c1e643 55 int vx, vy, deg;
lilac0112_1 28:8ac6c3c1e643 56 deg = (degree+5)/10;
lilac0112_1 28:8ac6c3c1e643 57 vx = power*sin(DEG2RAD(deg));
lilac0112_1 28:8ac6c3c1e643 58 vy = power*cos(DEG2RAD(deg));
lilac0112_1 28:8ac6c3c1e643 59 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 60 }
lilac0112_1 28:8ac6c3c1e643 61 void move_mouse(int32_t point[2], int vs){//三輪オムニの移動（マウスでの直交座標指定）
lilac0112_1 28:8ac6c3c1e643 62 int vx,vy;
lilac0112_1 28:8ac6c3c1e643 63 vx = (point[0] - data.mouse[0])/10000;
lilac0112_1 28:8ac6c3c1e643 64 vy = (point[1] - data.mouse[1])/10000;
lilac0112_1 28:8ac6c3c1e643 65 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 66 }
lilac0112_1 28:8ac6c3c1e643 67 //solenoid
lilac0112_1 28:8ac6c3c1e643 68 void AvailableSolenoid(void){
lilac0112_1 28:8ac6c3c1e643 69 if(data.KickFlag==0){
lilac0112_1 28:8ac6c3c1e643 70 data.KickFlag = 1;
lilac0112_1 28:8ac6c3c1e643 71 }
lilac0112_1 28:8ac6c3c1e643 72 }
lilac0112_1 28:8ac6c3c1e643 73 void DriveSolenoid(void){
lilac0112_1 28:8ac6c3c1e643 74 data.KickFlag = 0;
lilac0112_1 28:8ac6c3c1e643 75 kicker=1;
lilac0112_1 45:c23f25c00d0d 76 Solenoid_timeout.attach(&SolenoidSignal, .5);
lilac0112_1 28:8ac6c3c1e643 77 }
lilac0112_1 28:8ac6c3c1e643 78 void SolenoidSignal(void){
lilac0112_1 28:8ac6c3c1e643 79 kicker=0;
lilac0112_1 28:8ac6c3c1e643 80 }