CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@30:26070ba1f21f, 2016-02-01 (annotated)

Committer:
lilac0112_1
Date:
Mon Feb 01 11:43:58 2016 +0000
Revision:
30:26070ba1f21f
Parent:
29:b413b0bb07a1
Child:
31:0b0f64831771
chase&turn

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 19:342da3a5a474 3
lilac0112_1 21:d69a8f3c76e1 4 //Atk
lilac0112_1 21:d69a8f3c76e1 5 void modeAttack0(void){
lilac0112_1 28:8ac6c3c1e643 6 double ir_x, ir_y;
lilac0112_1 27:769cb5a7ea37 7 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 8 uint8_t LineDir[4];
lilac0112_1 27:769cb5a7ea37 9 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 10 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 11 ReadIr();
lilac0112_1 28:8ac6c3c1e643 12 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 13 }
lilac0112_1 28:8ac6c3c1e643 14 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 15 Line_ticker.detach();
lilac0112_1 28:8ac6c3c1e643 16 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 17 Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 28:8ac6c3c1e643 18 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 19 }
lilac0112_1 28:8ac6c3c1e643 20 ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 21 ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 22 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 23 ir_x *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 24 ir_y *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 25 }
lilac0112_1 28:8ac6c3c1e643 26 else{
lilac0112_1 28:8ac6c3c1e643 27 ir_x *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 28 ir_y *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 29 }
lilac0112_1 29:b413b0bb07a1 30 //Lineを考慮していないIrのみの値
lilac0112_1 29:b413b0bb07a1 31 vx = ir_x;
lilac0112_1 29:b413b0bb07a1 32 vy = ir_y;
lilac0112_1 29:b413b0bb07a1 33 //Line検出方向を調べる
lilac0112_1 28:8ac6c3c1e643 34 LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
lilac0112_1 28:8ac6c3c1e643 35 LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 28:8ac6c3c1e643 36 LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 37 LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
lilac0112_1 28:8ac6c3c1e643 38
lilac0112_1 28:8ac6c3c1e643 39 LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
lilac0112_1 28:8ac6c3c1e643 40 LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
lilac0112_1 28:8ac6c3c1e643 41
lilac0112_1 28:8ac6c3c1e643 42 vs = data.OutputPID;
lilac0112_1 29:b413b0bb07a1 43 /*
lilac0112_1 29:b413b0bb07a1 44 //ForLineAll
lilac0112_1 29:b413b0bb07a1 45 data.motorlog[X_AXIS] *= LineStop[X_AXIS];
lilac0112_1 29:b413b0bb07a1 46 data.motorlog[Y_AXIS] *= LineStop[Y_AXIS];
lilac0112_1 29:b413b0bb07a1 47 data.motorlog[X_AXIS] += vx;
lilac0112_1 29:b413b0bb07a1 48 data.motorlog[Y_AXIS] += vy;
lilac0112_1 29:b413b0bb07a1 49 */
lilac0112_1 28:8ac6c3c1e643 50 move(
lilac0112_1 29:b413b0bb07a1 51 vx*LineStop[X_AXIS] + (LINE_RF)*(vy!=0)*((-1)*(LineDir[A_SPOT]==0) + (LineDir[B_SPOT]==0)),
lilac0112_1 29:b413b0bb07a1 52 vy*LineStop[Y_AXIS] + (LINE_RF)*(vx!=0)*((LineDir[C_SPOT]==0) + (-1)*(LineDir[AB_SPOT]==0)),
lilac0112_1 28:8ac6c3c1e643 53 vs
lilac0112_1 28:8ac6c3c1e643 54 );
lilac0112_1 28:8ac6c3c1e643 55 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 56 tx_motor();
lilac0112_1 28:8ac6c3c1e643 57 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 58 }
lilac0112_1 28:8ac6c3c1e643 59 return;
lilac0112_1 28:8ac6c3c1e643 60 }
lilac0112_1 28:8ac6c3c1e643 61 void modeAttack1(void){
lilac0112_1 28:8ac6c3c1e643 62 return;
lilac0112_1 28:8ac6c3c1e643 63 }
lilac0112_1 28:8ac6c3c1e643 64 void modeAttack2(void){
lilac0112_1 28:8ac6c3c1e643 65 return;
lilac0112_1 28:8ac6c3c1e643 66 }
lilac0112_1 28:8ac6c3c1e643 67 void modeAttack3(void){
lilac0112_1 28:8ac6c3c1e643 68 return;
lilac0112_1 28:8ac6c3c1e643 69 }
lilac0112_1 28:8ac6c3c1e643 70 void modeAttack4(void){
lilac0112_1 28:8ac6c3c1e643 71 return;
lilac0112_1 28:8ac6c3c1e643 72 }
lilac0112_1 28:8ac6c3c1e643 73 void modeAttack5(void){
lilac0112_1 28:8ac6c3c1e643 74 return;
lilac0112_1 28:8ac6c3c1e643 75 }
lilac0112_1 28:8ac6c3c1e643 76 //Debug
lilac0112_1 28:8ac6c3c1e643 77 void modeDebug0(void){
lilac0112_1 28:8ac6c3c1e643 78 return;
lilac0112_1 28:8ac6c3c1e643 79 }
lilac0112_1 28:8ac6c3c1e643 80 void modeDebug1(void){//ChaseOnly NonLine
lilac0112_1 28:8ac6c3c1e643 81 int vx,vy,vs;
lilac0112_1 24:9fb74ea3c25d 82 if(data.IrFlag==1){
lilac0112_1 24:9fb74ea3c25d 83 ReadIr();
lilac0112_1 24:9fb74ea3c25d 84 data.IrFlag=0;
lilac0112_1 24:9fb74ea3c25d 85 }
lilac0112_1 27:769cb5a7ea37 86 if(data.PidFlag==1){
lilac0112_1 27:769cb5a7ea37 87 PidUpdate();
lilac0112_1 27:769cb5a7ea37 88 data.PidFlag=0;
lilac0112_1 27:769cb5a7ea37 89 }
lilac0112_1 28:8ac6c3c1e643 90
lilac0112_1 28:8ac6c3c1e643 91 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 92 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 93 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 94 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 95 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 96 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 97 }
lilac0112_1 28:8ac6c3c1e643 98 else{
lilac0112_1 28:8ac6c3c1e643 99 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 100 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 101 }
lilac0112_1 28:8ac6c3c1e643 102
lilac0112_1 28:8ac6c3c1e643 103
lilac0112_1 28:8ac6c3c1e643 104 move(vx, vy, vs);
lilac0112_1 28:8ac6c3c1e643 105
lilac0112_1 28:8ac6c3c1e643 106 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 107 tx_motor();
lilac0112_1 28:8ac6c3c1e643 108 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 109 }
lilac0112_1 28:8ac6c3c1e643 110 return;
lilac0112_1 28:8ac6c3c1e643 111 }
lilac0112_1 28:8ac6c3c1e643 112 void modeDebug2(void){//LineRestoringForce
lilac0112_1 28:8ac6c3c1e643 113 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 114 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 115 int LineForce[2];
lilac0112_1 28:8ac6c3c1e643 116 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 117 ReadIr();
lilac0112_1 28:8ac6c3c1e643 118 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 119 }
lilac0112_1 28:8ac6c3c1e643 120 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 121 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 122 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 123 }
lilac0112_1 27:769cb5a7ea37 124 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 27:769cb5a7ea37 125 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 27:769cb5a7ea37 126 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 127 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 128 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 129 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 130 }
lilac0112_1 28:8ac6c3c1e643 131 else{
lilac0112_1 28:8ac6c3c1e643 132 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 133 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 134 }
lilac0112_1 28:8ac6c3c1e643 135
lilac0112_1 28:8ac6c3c1e643 136
lilac0112_1 28:8ac6c3c1e643 137 LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 138 LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
lilac0112_1 28:8ac6c3c1e643 139
lilac0112_1 28:8ac6c3c1e643 140 LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
lilac0112_1 28:8ac6c3c1e643 141 if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 142 LineForce[X_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 143 }
lilac0112_1 28:8ac6c3c1e643 144 if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 145 LineForce[X_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 146 }
lilac0112_1 28:8ac6c3c1e643 147 if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
lilac0112_1 28:8ac6c3c1e643 148 LineForce[Y_AXIS] = -LINE_RF;
lilac0112_1 28:8ac6c3c1e643 149 }
lilac0112_1 28:8ac6c3c1e643 150 if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
lilac0112_1 28:8ac6c3c1e643 151 LineForce[Y_AXIS] = LINE_RF;
lilac0112_1 28:8ac6c3c1e643 152 }
lilac0112_1 28:8ac6c3c1e643 153
lilac0112_1 28:8ac6c3c1e643 154 move(
lilac0112_1 28:8ac6c3c1e643 155 vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 28:8ac6c3c1e643 156 vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 28:8ac6c3c1e643 157 vs
lilac0112_1 28:8ac6c3c1e643 158 );
lilac0112_1 28:8ac6c3c1e643 159 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 160 tx_motor();
lilac0112_1 28:8ac6c3c1e643 161 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 162 }
lilac0112_1 28:8ac6c3c1e643 163 return;
lilac0112_1 28:8ac6c3c1e643 164 }
lilac0112_1 28:8ac6c3c1e643 165 void modeDebug3(void){//movesum
lilac0112_1 28:8ac6c3c1e643 166 int vx,vy,vs;
lilac0112_1 28:8ac6c3c1e643 167 uint8_t LineStop[2];
lilac0112_1 28:8ac6c3c1e643 168 static uint8_t moveLnFlag[4]={1, 1, 1, 1};
lilac0112_1 28:8ac6c3c1e643 169 static int moveLnlog[4];
lilac0112_1 28:8ac6c3c1e643 170 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 171 ReadIr();
lilac0112_1 28:8ac6c3c1e643 172 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 173 }
lilac0112_1 28:8ac6c3c1e643 174 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 175 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 176 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 177 }
lilac0112_1 28:8ac6c3c1e643 178 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 28:8ac6c3c1e643 179 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 28:8ac6c3c1e643 180 vs = data.OutputPID;
lilac0112_1 28:8ac6c3c1e643 181 if(data.irPosition<8){
lilac0112_1 28:8ac6c3c1e643 182 vx *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 183 vy *= data.l_pow;
lilac0112_1 28:8ac6c3c1e643 184 }
lilac0112_1 28:8ac6c3c1e643 185 else{
lilac0112_1 28:8ac6c3c1e643 186 vx *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 187 vy *= data.s_pow;
lilac0112_1 28:8ac6c3c1e643 188 }
lilac0112_1 27:769cb5a7ea37 189
lilac0112_1 27:769cb5a7ea37 190 data.motorlog[X_AXIS] += vx;
lilac0112_1 27:769cb5a7ea37 191 data.motorlog[Y_AXIS] += vy;
lilac0112_1 27:769cb5a7ea37 192
lilac0112_1 27:769cb5a7ea37 193 if((data.lnFlag[A_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 194 moveLnFlag[A_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 195 moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 196 }
lilac0112_1 27:769cb5a7ea37 197 if((data.lnFlag[B_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 198 moveLnFlag[B_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 199 moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 200 }
lilac0112_1 27:769cb5a7ea37 201 if((data.lnFlag[C_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 202 moveLnFlag[C_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 203 moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 204 }
lilac0112_1 27:769cb5a7ea37 205 if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
lilac0112_1 27:769cb5a7ea37 206 moveLnFlag[AB_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 207 moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 208 }
lilac0112_1 27:769cb5a7ea37 209
lilac0112_1 27:769cb5a7ea37 210 if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 211 if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 212 if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 213 if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 214
lilac0112_1 27:769cb5a7ea37 215 LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
lilac0112_1 27:769cb5a7ea37 216 LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
lilac0112_1 26:fbb03281fc7d 217
lilac0112_1 27:769cb5a7ea37 218 move(
lilac0112_1 27:769cb5a7ea37 219 vx*LineStop[X_AXIS],
lilac0112_1 27:769cb5a7ea37 220 vy*LineStop[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 221 vs
lilac0112_1 26:fbb03281fc7d 222 );
lilac0112_1 27:769cb5a7ea37 223 if(data.MotorFlag==1){
lilac0112_1 27:769cb5a7ea37 224 tx_motor();
lilac0112_1 27:769cb5a7ea37 225 data.MotorFlag=0;
lilac0112_1 27:769cb5a7ea37 226 }
lilac0112_1 21:d69a8f3c76e1 227 return;
lilac0112_1 21:d69a8f3c76e1 228 }
lilac0112_1 28:8ac6c3c1e643 229 void modeDebug4(void){//solenoid
lilac0112_1 28:8ac6c3c1e643 230 if(data.KickFlag==1){
lilac0112_1 28:8ac6c3c1e643 231 DriveSolenoid();
lilac0112_1 28:8ac6c3c1e643 232 }
lilac0112_1 21:d69a8f3c76e1 233 return;
lilac0112_1 21:d69a8f3c76e1 234 }
lilac0112_1 28:8ac6c3c1e643 235 void modeDebug5(void){//cmpsTest
lilac0112_1 28:8ac6c3c1e643 236 if(data.IrFlag==1){
lilac0112_1 28:8ac6c3c1e643 237 ReadIr();
lilac0112_1 28:8ac6c3c1e643 238 data.IrFlag=0;
lilac0112_1 28:8ac6c3c1e643 239 }
lilac0112_1 28:8ac6c3c1e643 240 if(data.PidFlag==1){
lilac0112_1 28:8ac6c3c1e643 241 PidUpdate();
lilac0112_1 28:8ac6c3c1e643 242 data.PidFlag=0;
lilac0112_1 28:8ac6c3c1e643 243 }
lilac0112_1 28:8ac6c3c1e643 244 move(0,0,data.OutputPID);
lilac0112_1 28:8ac6c3c1e643 245 if(data.MotorFlag==1){
lilac0112_1 28:8ac6c3c1e643 246 tx_motor();
lilac0112_1 28:8ac6c3c1e643 247 data.MotorFlag=0;
lilac0112_1 28:8ac6c3c1e643 248 }
lilac0112_1 28:8ac6c3c1e643 249
lilac0112_1 19:342da3a5a474 250 return;
lilac0112_1 19:342da3a5a474 251 }