![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |