CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T_2v00

Main Program

Dependencies:   mbed AQM1602 HMC6352 PID

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

Committer:
lilac0112_1
Date:
Thu Feb 25 06:12:47 2016 +0000
Revision:
45:c23f25c00d0d
Parent:
42:02aaa806d929 
Hokushinetsu(20:1)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 42:02aaa806d929 1 #include "mbed.h"
lilac0112_1 42:02aaa806d929 2 #include "extern.h"
lilac0112_1 42:02aaa806d929 3
lilac0112_1 42:02aaa806d929 4 //Debug
lilac0112_1 42:02aaa806d929 5 void modeDebug0(void){
lilac0112_1 42:02aaa806d929 6 return;
lilac0112_1 42:02aaa806d929 7 }
lilac0112_1 42:02aaa806d929 8 void modeDebug1(void){//ChaseOnly NonLine
lilac0112_1 42:02aaa806d929 9 int vx,vy,vs;
lilac0112_1 42:02aaa806d929 10 if(data.IrFlag==1){
lilac0112_1 42:02aaa806d929 11 ReadIr();
lilac0112_1 42:02aaa806d929 12 data.IrFlag=0;
lilac0112_1 42:02aaa806d929 13 }
lilac0112_1 42:02aaa806d929 14 if(data.PidFlag==1){
lilac0112_1 42:02aaa806d929 15 PidUpdate();
lilac0112_1 42:02aaa806d929 16 data.PidFlag=0;
lilac0112_1 42:02aaa806d929 17 }
lilac0112_1 42:02aaa806d929 18
lilac0112_1 42:02aaa806d929 19 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 42:02aaa806d929 20 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 42:02aaa806d929 21 vs = data.OutputPID;
lilac0112_1 42:02aaa806d929 22 if(data.irPosition<8){
lilac0112_1 42:02aaa806d929 23 vx *= data.l_pow;
lilac0112_1 42:02aaa806d929 24 vy *= data.l_pow;
lilac0112_1 42:02aaa806d929 25 }
lilac0112_1 42:02aaa806d929 26 else{
lilac0112_1 42:02aaa806d929 27 vx *= data.s_pow;
lilac0112_1 42:02aaa806d929 28 vy *= data.s_pow;
lilac0112_1 42:02aaa806d929 29 }
lilac0112_1 42:02aaa806d929 30
lilac0112_1 42:02aaa806d929 31
lilac0112_1 42:02aaa806d929 32 move(vx, vy, vs);
lilac0112_1 42:02aaa806d929 33
lilac0112_1 42:02aaa806d929 34 if(data.MotorFlag==1){
lilac0112_1 42:02aaa806d929 35 tx_motor();
lilac0112_1 42:02aaa806d929 36 data.MotorFlag=0;
lilac0112_1 42:02aaa806d929 37 }
lilac0112_1 42:02aaa806d929 38 return;
lilac0112_1 42:02aaa806d929 39 }
lilac0112_1 42:02aaa806d929 40 void modeDebug2(void){//LineRestoringForce
lilac0112_1 42:02aaa806d929 41 int vx,vy,vs;
lilac0112_1 42:02aaa806d929 42 uint8_t LineStop[2];
lilac0112_1 42:02aaa806d929 43 int LineForce[2];
lilac0112_1 42:02aaa806d929 44 if(data.IrFlag==1){
lilac0112_1 42:02aaa806d929 45 ReadIr();
lilac0112_1 42:02aaa806d929 46 data.IrFlag=0;
lilac0112_1 42:02aaa806d929 47 }
lilac0112_1 42:02aaa806d929 48 if(data.PidFlag==1){
lilac0112_1 42:02aaa806d929 49 PidUpdate();
lilac0112_1 42:02aaa806d929 50 data.PidFlag=0;
lilac0112_1 42:02aaa806d929 51 }
lilac0112_1 42:02aaa806d929 52 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 42:02aaa806d929 53 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 42:02aaa806d929 54 vs = data.OutputPID;
lilac0112_1 42:02aaa806d929 55 if(data.irPosition<8){
lilac0112_1 42:02aaa806d929 56 vx *= data.l_pow;
lilac0112_1 42:02aaa806d929 57 vy *= data.l_pow;
lilac0112_1 42:02aaa806d929 58 }
lilac0112_1 42:02aaa806d929 59 else{
lilac0112_1 42:02aaa806d929 60 vx *= data.s_pow;
lilac0112_1 42:02aaa806d929 61 vy *= data.s_pow;
lilac0112_1 42:02aaa806d929 62 }
lilac0112_1 42:02aaa806d929 63
lilac0112_1 42:02aaa806d929 64
lilac0112_1 42:02aaa806d929 65 LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
lilac0112_1 42:02aaa806d929 66 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 42:02aaa806d929 67
lilac0112_1 42:02aaa806d929 68 LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
lilac0112_1 42:02aaa806d929 69 if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
lilac0112_1 42:02aaa806d929 70 LineForce[X_AXIS] = -LINE_RF;
lilac0112_1 42:02aaa806d929 71 }
lilac0112_1 42:02aaa806d929 72 if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
lilac0112_1 42:02aaa806d929 73 LineForce[X_AXIS] = LINE_RF;
lilac0112_1 42:02aaa806d929 74 }
lilac0112_1 42:02aaa806d929 75 if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
lilac0112_1 42:02aaa806d929 76 LineForce[Y_AXIS] = -LINE_RF;
lilac0112_1 42:02aaa806d929 77 }
lilac0112_1 42:02aaa806d929 78 if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
lilac0112_1 42:02aaa806d929 79 LineForce[Y_AXIS] = LINE_RF;
lilac0112_1 42:02aaa806d929 80 }
lilac0112_1 42:02aaa806d929 81
lilac0112_1 42:02aaa806d929 82 move(
lilac0112_1 42:02aaa806d929 83 vx*LineStop[X_AXIS] + LineForce[X_AXIS],
lilac0112_1 42:02aaa806d929 84 vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
lilac0112_1 42:02aaa806d929 85 vs
lilac0112_1 42:02aaa806d929 86 );
lilac0112_1 42:02aaa806d929 87 if(data.MotorFlag==1){
lilac0112_1 42:02aaa806d929 88 tx_motor();
lilac0112_1 42:02aaa806d929 89 data.MotorFlag=0;
lilac0112_1 42:02aaa806d929 90 }
lilac0112_1 42:02aaa806d929 91 return;
lilac0112_1 42:02aaa806d929 92 }
lilac0112_1 42:02aaa806d929 93 void modeDebug3(void){//movesum
lilac0112_1 42:02aaa806d929 94 int vx,vy,vs;
lilac0112_1 42:02aaa806d929 95 uint8_t LineStop[2];
lilac0112_1 42:02aaa806d929 96 static uint8_t moveLnFlag[4]={1, 1, 1, 1};
lilac0112_1 42:02aaa806d929 97 static int moveLnlog[4];
lilac0112_1 42:02aaa806d929 98 if(data.IrFlag==1){
lilac0112_1 42:02aaa806d929 99 ReadIr();
lilac0112_1 42:02aaa806d929 100 data.IrFlag=0;
lilac0112_1 42:02aaa806d929 101 }
lilac0112_1 42:02aaa806d929 102 if(data.PidFlag==1){
lilac0112_1 42:02aaa806d929 103 PidUpdate();
lilac0112_1 42:02aaa806d929 104 data.PidFlag=0;
lilac0112_1 42:02aaa806d929 105 }
lilac0112_1 42:02aaa806d929 106 vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
lilac0112_1 42:02aaa806d929 107 vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
lilac0112_1 42:02aaa806d929 108 vs = data.OutputPID;
lilac0112_1 42:02aaa806d929 109 if(data.irPosition<8){
lilac0112_1 42:02aaa806d929 110 vx *= data.l_pow;
lilac0112_1 42:02aaa806d929 111 vy *= data.l_pow;
lilac0112_1 42:02aaa806d929 112 }
lilac0112_1 42:02aaa806d929 113 else{
lilac0112_1 42:02aaa806d929 114 vx *= data.s_pow;
lilac0112_1 42:02aaa806d929 115 vy *= data.s_pow;
lilac0112_1 42:02aaa806d929 116 }
lilac0112_1 42:02aaa806d929 117
lilac0112_1 42:02aaa806d929 118 data.motorlog[X_AXIS] += vx;
lilac0112_1 42:02aaa806d929 119 data.motorlog[Y_AXIS] += vy;
lilac0112_1 42:02aaa806d929 120
lilac0112_1 42:02aaa806d929 121 if((data.lnFlag[A_SPOT]==1)&&(1)){
lilac0112_1 42:02aaa806d929 122 moveLnFlag[A_SPOT]=0;
lilac0112_1 42:02aaa806d929 123 moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 42:02aaa806d929 124 }
lilac0112_1 42:02aaa806d929 125 if((data.lnFlag[B_SPOT]==1)&&(1)){
lilac0112_1 42:02aaa806d929 126 moveLnFlag[B_SPOT]=0;
lilac0112_1 42:02aaa806d929 127 moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
lilac0112_1 42:02aaa806d929 128 }
lilac0112_1 42:02aaa806d929 129 if((data.lnFlag[C_SPOT]==1)&&(1)){
lilac0112_1 42:02aaa806d929 130 moveLnFlag[C_SPOT]=0;
lilac0112_1 42:02aaa806d929 131 moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 42:02aaa806d929 132 }
lilac0112_1 42:02aaa806d929 133 if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
lilac0112_1 42:02aaa806d929 134 moveLnFlag[AB_SPOT]=0;
lilac0112_1 42:02aaa806d929 135 moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
lilac0112_1 42:02aaa806d929 136 }
lilac0112_1 42:02aaa806d929 137
lilac0112_1 42:02aaa806d929 138 if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
lilac0112_1 42:02aaa806d929 139 if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
lilac0112_1 42:02aaa806d929 140 if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
lilac0112_1 42:02aaa806d929 141 if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
lilac0112_1 42:02aaa806d929 142
lilac0112_1 42:02aaa806d929 143 LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
lilac0112_1 42:02aaa806d929 144 LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
lilac0112_1 42:02aaa806d929 145
lilac0112_1 42:02aaa806d929 146 move(
lilac0112_1 42:02aaa806d929 147 vx*LineStop[X_AXIS],
lilac0112_1 42:02aaa806d929 148 vy*LineStop[Y_AXIS],
lilac0112_1 42:02aaa806d929 149 vs
lilac0112_1 42:02aaa806d929 150 );
lilac0112_1 42:02aaa806d929 151 if(data.MotorFlag==1){
lilac0112_1 42:02aaa806d929 152 tx_motor();
lilac0112_1 42:02aaa806d929 153 data.MotorFlag=0;
lilac0112_1 42:02aaa806d929 154 }
lilac0112_1 42:02aaa806d929 155 return;
lilac0112_1 42:02aaa806d929 156 }
lilac0112_1 42:02aaa806d929 157 void modeDebug4(void){//solenoid
lilac0112_1 42:02aaa806d929 158 if(data.KickFlag==1){
lilac0112_1 42:02aaa806d929 159 DriveSolenoid();
lilac0112_1 42:02aaa806d929 160 }
lilac0112_1 42:02aaa806d929 161 return;
lilac0112_1 42:02aaa806d929 162 }
lilac0112_1 42:02aaa806d929 163 void modeDebug5(void){//cmpsTest
lilac0112_1 42:02aaa806d929 164 if(data.IrFlag==1){
lilac0112_1 42:02aaa806d929 165 ReadIr();
lilac0112_1 42:02aaa806d929 166 data.IrFlag=0;
lilac0112_1 42:02aaa806d929 167 }
lilac0112_1 42:02aaa806d929 168 if(data.PidFlag==1){
lilac0112_1 42:02aaa806d929 169 PidUpdate();
lilac0112_1 42:02aaa806d929 170 data.PidFlag=0;
lilac0112_1 42:02aaa806d929 171 }
lilac0112_1 45:c23f25c00d0d 172 move(0,0,10);
lilac0112_1 45:c23f25c00d0d 173 LED[0]=1;
lilac0112_1 45:c23f25c00d0d 174 LED[3]=1;
lilac0112_1 42:02aaa806d929 175 if(data.MotorFlag==1){
lilac0112_1 42:02aaa806d929 176 tx_motor();
lilac0112_1 42:02aaa806d929 177 data.MotorFlag=0;
lilac0112_1 42:02aaa806d929 178 }
lilac0112_1 42:02aaa806d929 179
lilac0112_1 42:02aaa806d929 180 return;
lilac0112_1 42:02aaa806d929 181 }