CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/strategy/strategy.cpp@27:769cb5a7ea37, 2016-01-31 (annotated)

Committer:
lilac0112_1
Date:
Sun Jan 31 15:46:19 2016 +0000
Revision:
27:769cb5a7ea37
Parent:
26:fbb03281fc7d
Child:
28:8ac6c3c1e643
plan; solenoid; ir_turn; line stop

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 27:769cb5a7ea37 6 int vx,vy,vs;
lilac0112_1 27:769cb5a7ea37 7 uint8_t LineStop[2];
lilac0112_1 27:769cb5a7ea37 8 static uint8_t moveLnFlag[4]={1, 1, 1, 1};
lilac0112_1 27:769cb5a7ea37 9 static int moveLnlog[4];
lilac0112_1 27:769cb5a7ea37 10 //int LineForce[2];
lilac0112_1 27:769cb5a7ea37 11 //int Return;
lilac0112_1 27:769cb5a7ea37 12 //int line
lilac0112_1 24:9fb74ea3c25d 13 if(data.IrFlag==1){
lilac0112_1 24:9fb74ea3c25d 14 ReadIr();
lilac0112_1 24:9fb74ea3c25d 15 data.IrFlag=0;
lilac0112_1 24:9fb74ea3c25d 16 }
lilac0112_1 27:769cb5a7ea37 17 if(data.PidFlag==1){
lilac0112_1 27:769cb5a7ea37 18 PidUpdate();
lilac0112_1 27:769cb5a7ea37 19 data.PidFlag=0;
lilac0112_1 27:769cb5a7ea37 20 }
lilac0112_1 25:f98a7c2a5970 21 //move(30,30,data.OutputPID);
lilac0112_1 26:fbb03281fc7d 22 //move(30,0,data.OutputPID);
lilac0112_1 25:f98a7c2a5970 23 /*move(
lilac0112_1 24:9fb74ea3c25d 24 ir_move_val[data.irNotice][data.irPosition][IR_X],
lilac0112_1 24:9fb74ea3c25d 25 ir_move_val[data.irNotice][data.irPosition][IR_Y],
lilac0112_1 24:9fb74ea3c25d 26 data.OutputPID
lilac0112_1 25:f98a7c2a5970 27 );*/
lilac0112_1 27:769cb5a7ea37 28 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 27:769cb5a7ea37 29 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 27:769cb5a7ea37 30 vs = data.OutputPID;
lilac0112_1 27:769cb5a7ea37 31
lilac0112_1 27:769cb5a7ea37 32 data.motorlog[X_AXIS] += vx;
lilac0112_1 27:769cb5a7ea37 33 data.motorlog[Y_AXIS] += vy;
lilac0112_1 27:769cb5a7ea37 34
lilac0112_1 27:769cb5a7ea37 35 if((data.lnFlag[A_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 36 moveLnFlag[A_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 37 moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 38 }
lilac0112_1 27:769cb5a7ea37 39 if((data.lnFlag[B_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 40 moveLnFlag[B_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 41 moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 27:769cb5a7ea37 42 }
lilac0112_1 27:769cb5a7ea37 43 if((data.lnFlag[C_SPOT]==1)&&(1)){
lilac0112_1 27:769cb5a7ea37 44 moveLnFlag[C_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 45 moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 46 }
lilac0112_1 27:769cb5a7ea37 47 if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
lilac0112_1 27:769cb5a7ea37 48 moveLnFlag[AB_SPOT]=0;
lilac0112_1 27:769cb5a7ea37 49 moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 27:769cb5a7ea37 50 }
lilac0112_1 27:769cb5a7ea37 51
lilac0112_1 27:769cb5a7ea37 52 if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 53 if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 54 if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 55 if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
lilac0112_1 27:769cb5a7ea37 56
lilac0112_1 27:769cb5a7ea37 57 //LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
lilac0112_1 27:769cb5a7ea37 58 //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 27:769cb5a7ea37 59
lilac0112_1 27:769cb5a7ea37 60 LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
lilac0112_1 27:769cb5a7ea37 61 LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
lilac0112_1 26:fbb03281fc7d 62
lilac0112_1 27:769cb5a7ea37 63 move(
lilac0112_1 27:769cb5a7ea37 64 vx*LineStop[X_AXIS],
lilac0112_1 27:769cb5a7ea37 65 vy*LineStop[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 66 vs
lilac0112_1 26:fbb03281fc7d 67 );
lilac0112_1 27:769cb5a7ea37 68 /*
lilac0112_1 27:769cb5a7ea37 69 LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
lilac0112_1 27:769cb5a7ea37 70 if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
lilac0112_1 27:769cb5a7ea37 71 LineForce[X_AXIS] = -LINE_RF;
lilac0112_1 27:769cb5a7ea37 72 }
lilac0112_1 27:769cb5a7ea37 73 if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
lilac0112_1 27:769cb5a7ea37 74 LineForce[X_AXIS] = LINE_RF;
lilac0112_1 27:769cb5a7ea37 75 }
lilac0112_1 27:769cb5a7ea37 76 if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
lilac0112_1 27:769cb5a7ea37 77 LineForce[Y_AXIS] = -LINE_RF;
lilac0112_1 27:769cb5a7ea37 78 }
lilac0112_1 27:769cb5a7ea37 79 if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
lilac0112_1 27:769cb5a7ea37 80 LineForce[Y_AXIS] = LINE_RF;
lilac0112_1 27:769cb5a7ea37 81 }
lilac0112_1 27:769cb5a7ea37 82
lilac0112_1 27:769cb5a7ea37 83 move(
lilac0112_1 27:769cb5a7ea37 84 vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 27:769cb5a7ea37 85 vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 86 vs
lilac0112_1 27:769cb5a7ea37 87 );*/
lilac0112_1 27:769cb5a7ea37 88
lilac0112_1 27:769cb5a7ea37 89 /*
lilac0112_1 27:769cb5a7ea37 90 move(
lilac0112_1 27:769cb5a7ea37 91 0,//vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 27:769cb5a7ea37 92 0,//vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 27:769cb5a7ea37 93 vs
lilac0112_1 27:769cb5a7ea37 94 );
lilac0112_1 27:769cb5a7ea37 95 */
lilac0112_1 27:769cb5a7ea37 96 if(data.MotorFlag==1){
lilac0112_1 27:769cb5a7ea37 97 tx_motor();
lilac0112_1 27:769cb5a7ea37 98 data.MotorFlag=0;
lilac0112_1 27:769cb5a7ea37 99 }
lilac0112_1 24:9fb74ea3c25d 100 //move(0,0,data.OutputPID);
lilac0112_1 21:d69a8f3c76e1 101 return;
lilac0112_1 21:d69a8f3c76e1 102 }
lilac0112_1 21:d69a8f3c76e1 103 void modeAttack1(void){
lilac0112_1 21:d69a8f3c76e1 104 return;
lilac0112_1 21:d69a8f3c76e1 105 }
lilac0112_1 21:d69a8f3c76e1 106 void modeAttack2(void){
lilac0112_1 21:d69a8f3c76e1 107 return;
lilac0112_1 21:d69a8f3c76e1 108 }
lilac0112_1 21:d69a8f3c76e1 109 void modeAttack3(void){
lilac0112_1 21:d69a8f3c76e1 110 return;
lilac0112_1 21:d69a8f3c76e1 111 }
lilac0112_1 21:d69a8f3c76e1 112 void modeAttack4(void){
lilac0112_1 21:d69a8f3c76e1 113 return;
lilac0112_1 21:d69a8f3c76e1 114 }
lilac0112_1 21:d69a8f3c76e1 115 void modeAttack5(void){
lilac0112_1 21:d69a8f3c76e1 116 return;
lilac0112_1 21:d69a8f3c76e1 117 }
lilac0112_1 21:d69a8f3c76e1 118 //Debug
lilac0112_1 19:342da3a5a474 119 void modeDebug0(void){
lilac0112_1 19:342da3a5a474 120 return;
lilac0112_1 19:342da3a5a474 121 }
lilac0112_1 19:342da3a5a474 122 void modeDebug1(void){
lilac0112_1 19:342da3a5a474 123 return;
lilac0112_1 19:342da3a5a474 124 }
lilac0112_1 19:342da3a5a474 125 void modeDebug2(void){
lilac0112_1 19:342da3a5a474 126 return;
lilac0112_1 19:342da3a5a474 127 }
lilac0112_1 19:342da3a5a474 128 void modeDebug3(void){
lilac0112_1 19:342da3a5a474 129 return;
lilac0112_1 19:342da3a5a474 130 }
lilac0112_1 21:d69a8f3c76e1 131 void modeDebug4(void){
lilac0112_1 19:342da3a5a474 132 return;
lilac0112_1 19:342da3a5a474 133 }
lilac0112_1 21:d69a8f3c76e1 134 void modeDebug5(void){
lilac0112_1 19:342da3a5a474 135 return;
lilac0112_1 19:342da3a5a474 136 }