robocup
Dependencies: HMC6352 PID mbed
main.cpp@0:13ab960fc61f, 2013-03-08 (annotated)
- Committer:
- akudohune
- Date:
- Fri Mar 08 07:13:29 2013 +0000
- Revision:
- 0:13ab960fc61f
ver1_2_0;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akudohune | 0:13ab960fc61f | 1 | #include <math.h> |
akudohune | 0:13ab960fc61f | 2 | #include <sstream> |
akudohune | 0:13ab960fc61f | 3 | #include "mbed.h" |
akudohune | 0:13ab960fc61f | 4 | #include "HMC6352.h" |
akudohune | 0:13ab960fc61f | 5 | #include "PID.h" |
akudohune | 0:13ab960fc61f | 6 | #include "main.h" |
akudohune | 0:13ab960fc61f | 7 | |
akudohune | 0:13ab960fc61f | 8 | |
akudohune | 0:13ab960fc61f | 9 | /*********** Serial interrupt ***********/ |
akudohune | 0:13ab960fc61f | 10 | |
akudohune | 0:13ab960fc61f | 11 | void Tx_interrupt() |
akudohune | 0:13ab960fc61f | 12 | { |
akudohune | 0:13ab960fc61f | 13 | array(speed[0],speed[1],speed[2],speed[3]); |
akudohune | 0:13ab960fc61f | 14 | driver.printf("%s",StringFIN.c_str()); |
akudohune | 0:13ab960fc61f | 15 | //pc.printf("%s",StringFIN.c_str()); |
akudohune | 0:13ab960fc61f | 16 | //pc.printf("compass.sample = %f\n",compass.sample() / 1.0); |
akudohune | 0:13ab960fc61f | 17 | } |
akudohune | 0:13ab960fc61f | 18 | /* |
akudohune | 0:13ab960fc61f | 19 | void Rx_interrupt() |
akudohune | 0:13ab960fc61f | 20 | { |
akudohune | 0:13ab960fc61f | 21 | if(driver.readable()){ |
akudohune | 0:13ab960fc61f | 22 | //pc.printf("%d\n",driver.getc()); |
akudohune | 0:13ab960fc61f | 23 | } |
akudohune | 0:13ab960fc61f | 24 | }*/ |
akudohune | 0:13ab960fc61f | 25 | |
akudohune | 0:13ab960fc61f | 26 | |
akudohune | 0:13ab960fc61f | 27 | /*********** Serial interrupt end **********/ |
akudohune | 0:13ab960fc61f | 28 | |
akudohune | 0:13ab960fc61f | 29 | void PidUpdata() |
akudohune | 0:13ab960fc61f | 30 | { |
akudohune | 0:13ab960fc61f | 31 | float deviation = 0; |
akudohune | 0:13ab960fc61f | 32 | |
akudohune | 0:13ab960fc61f | 33 | |
akudohune | 0:13ab960fc61f | 34 | if(standard < 180.0){ |
akudohune | 0:13ab960fc61f | 35 | if((compass.sample() / 10.0) < standard){ |
akudohune | 0:13ab960fc61f | 36 | inputPID = 180.0 -(standard - (compass.sample() / 10.0)); |
akudohune | 0:13ab960fc61f | 37 | }else if((compass.sample() / 10.0) < 180.0 + standard){ |
akudohune | 0:13ab960fc61f | 38 | inputPID = 180.0 +((compass.sample() / 10.0) - standard); |
akudohune | 0:13ab960fc61f | 39 | }else{ |
akudohune | 0:13ab960fc61f | 40 | inputPID = 180.0 - ((360.0 - (compass.sample() / 10.0)) + standard); |
akudohune | 0:13ab960fc61f | 41 | } |
akudohune | 0:13ab960fc61f | 42 | }else if(standard > 180.0){ |
akudohune | 0:13ab960fc61f | 43 | if((compass.sample() / 10.0) > standard){ |
akudohune | 0:13ab960fc61f | 44 | inputPID = 180.0 +((compass.sample() / 10.0) - standard); |
akudohune | 0:13ab960fc61f | 45 | }else if((compass.sample() / 10.0) > standard - 180.0){ |
akudohune | 0:13ab960fc61f | 46 | inputPID = 180.0 -(standard - (compass.sample() / 10.0)); |
akudohune | 0:13ab960fc61f | 47 | }else{ |
akudohune | 0:13ab960fc61f | 48 | inputPID = 180.0 + ((compass.sample() / 10.0) + (360.0 - standard)); |
akudohune | 0:13ab960fc61f | 49 | } |
akudohune | 0:13ab960fc61f | 50 | }else{ |
akudohune | 0:13ab960fc61f | 51 | inputPID = compass.sample() / 10.0; |
akudohune | 0:13ab960fc61f | 52 | } |
akudohune | 0:13ab960fc61f | 53 | |
akudohune | 0:13ab960fc61f | 54 | if(inputPID < 0){ |
akudohune | 0:13ab960fc61f | 55 | inputPID *= -1; // |
akudohune | 0:13ab960fc61f | 56 | } |
akudohune | 0:13ab960fc61f | 57 | |
akudohune | 0:13ab960fc61f | 58 | |
akudohune | 0:13ab960fc61f | 59 | //pid.setProcessValue(inputPID); |
akudohune | 0:13ab960fc61f | 60 | |
akudohune | 0:13ab960fc61f | 61 | deviation = (inputPID - 180.0); |
akudohune | 0:13ab960fc61f | 62 | |
akudohune | 0:13ab960fc61f | 63 | compassPID = ((deviation / 180.0) * 20.0 + ((deviation - lastData) / 2.0)); |
akudohune | 0:13ab960fc61f | 64 | |
akudohune | 0:13ab960fc61f | 65 | if(compassPID > 100)compassPID = 100; |
akudohune | 0:13ab960fc61f | 66 | else if(compassPID < -100)compassPID = -100; |
akudohune | 0:13ab960fc61f | 67 | |
akudohune | 0:13ab960fc61f | 68 | //pc.printf("%f\n",compassPID); |
akudohune | 0:13ab960fc61f | 69 | |
akudohune | 0:13ab960fc61f | 70 | lastData = deviation; |
akudohune | 0:13ab960fc61f | 71 | } |
akudohune | 0:13ab960fc61f | 72 | |
akudohune | 0:13ab960fc61f | 73 | |
akudohune | 0:13ab960fc61f | 74 | |
akudohune | 0:13ab960fc61f | 75 | void move(int vx, int vy, int vs) |
akudohune | 0:13ab960fc61f | 76 | { |
akudohune | 0:13ab960fc61f | 77 | double motVal[MOT_NUM] = {0}; |
akudohune | 0:13ab960fc61f | 78 | |
akudohune | 0:13ab960fc61f | 79 | motVal[0] = (double)((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)); |
akudohune | 0:13ab960fc61f | 80 | motVal[1] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)); |
akudohune | 0:13ab960fc61f | 81 | motVal[2] = (double)((-0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * -vs)); |
akudohune | 0:13ab960fc61f | 82 | motVal[3] = (double)((0.5 * vx) + ((sqrt(3.0) / 2.0) * vy) + (Long * vs)); |
akudohune | 0:13ab960fc61f | 83 | |
akudohune | 0:13ab960fc61f | 84 | for(uint8_t i = 0;i < MOT_NUM;i++){ |
akudohune | 0:13ab960fc61f | 85 | if(motVal[i] > 100)motVal[i] = 100; |
akudohune | 0:13ab960fc61f | 86 | else if(motVal[i] < -100)motVal[i] = -100; |
akudohune | 0:13ab960fc61f | 87 | speed[i] = motVal[i]; |
akudohune | 0:13ab960fc61f | 88 | } |
akudohune | 0:13ab960fc61f | 89 | /* |
akudohune | 0:13ab960fc61f | 90 | pc.printf("speed1 = %d\n",speed[0]); |
akudohune | 0:13ab960fc61f | 91 | pc.printf("speed2 = %d\n",speed[1]); |
akudohune | 0:13ab960fc61f | 92 | pc.printf("speed3 = %d\n",speed[2]); |
akudohune | 0:13ab960fc61f | 93 | pc.printf("speed4 = %d\n\n",speed[3]); |
akudohune | 0:13ab960fc61f | 94 | */ |
akudohune | 0:13ab960fc61f | 95 | ////pc.printf("%s",StringFIN.c_str()); |
akudohune | 0:13ab960fc61f | 96 | } |
akudohune | 0:13ab960fc61f | 97 | |
akudohune | 0:13ab960fc61f | 98 | void init() |
akudohune | 0:13ab960fc61f | 99 | { |
akudohune | 0:13ab960fc61f | 100 | |
akudohune | 0:13ab960fc61f | 101 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
akudohune | 0:13ab960fc61f | 102 | StartButton.mode(PullUp); |
akudohune | 0:13ab960fc61f | 103 | CalibEnterButton.mode(PullUp); |
akudohune | 0:13ab960fc61f | 104 | CalibExitButton.mode(PullUp); |
akudohune | 0:13ab960fc61f | 105 | driver.baud(BAUD_RATE); |
akudohune | 0:13ab960fc61f | 106 | wait_ms(MOTDRIVER_WAIT); |
akudohune | 0:13ab960fc61f | 107 | driver.printf("1F0002F0003F0004F000\r\n"); |
akudohune | 0:13ab960fc61f | 108 | |
akudohune | 0:13ab960fc61f | 109 | led1 = ON; |
akudohune | 0:13ab960fc61f | 110 | |
akudohune | 0:13ab960fc61f | 111 | while(StartButton){ |
akudohune | 0:13ab960fc61f | 112 | if(!CalibEnterButton){ |
akudohune | 0:13ab960fc61f | 113 | led1 = OFF; |
akudohune | 0:13ab960fc61f | 114 | led2 = ON; |
akudohune | 0:13ab960fc61f | 115 | compass.setCalibrationMode(ENTER); |
akudohune | 0:13ab960fc61f | 116 | while(CalibExitButton); |
akudohune | 0:13ab960fc61f | 117 | compass.setCalibrationMode(EXIT); |
akudohune | 0:13ab960fc61f | 118 | led2 = OFF; |
akudohune | 0:13ab960fc61f | 119 | led3 = ON; |
akudohune | 0:13ab960fc61f | 120 | } |
akudohune | 0:13ab960fc61f | 121 | } |
akudohune | 0:13ab960fc61f | 122 | |
akudohune | 0:13ab960fc61f | 123 | standard = compass.sample() / 10.0; |
akudohune | 0:13ab960fc61f | 124 | led1 = OFF; |
akudohune | 0:13ab960fc61f | 125 | led3 = OFF; |
akudohune | 0:13ab960fc61f | 126 | |
akudohune | 0:13ab960fc61f | 127 | pid.setInputLimits(0.0, 360.0); |
akudohune | 0:13ab960fc61f | 128 | pid.setOutputLimits(-50.0, 50.0); |
akudohune | 0:13ab960fc61f | 129 | pid.setBias(0.0); |
akudohune | 0:13ab960fc61f | 130 | pid.setMode(AUTO_MODE); |
akudohune | 0:13ab960fc61f | 131 | pid.setSetPoint(180.0); |
akudohune | 0:13ab960fc61f | 132 | |
akudohune | 0:13ab960fc61f | 133 | pidUpdata.attach(&PidUpdata, 0.06); |
akudohune | 0:13ab960fc61f | 134 | //ultrasonic.attach(&Ultrasonic, 0.1); |
akudohune | 0:13ab960fc61f | 135 | IR.attach(&IR_Position,0.04); |
akudohune | 0:13ab960fc61f | 136 | driver.attach(&Tx_interrupt, Serial::TxIrq); |
akudohune | 0:13ab960fc61f | 137 | //driver.attach(&Rx_interrupt, Serial::RxIrq); |
akudohune | 0:13ab960fc61f | 138 | |
akudohune | 0:13ab960fc61f | 139 | timer2.start(); |
akudohune | 0:13ab960fc61f | 140 | } |
akudohune | 0:13ab960fc61f | 141 | |
akudohune | 0:13ab960fc61f | 142 | int main() |
akudohune | 0:13ab960fc61f | 143 | { |
akudohune | 0:13ab960fc61f | 144 | int vx=0,vy=0,vs=0; |
akudohune | 0:13ab960fc61f | 145 | |
akudohune | 0:13ab960fc61f | 146 | init(); |
akudohune | 0:13ab960fc61f | 147 | |
akudohune | 0:13ab960fc61f | 148 | while(1) { |
akudohune | 0:13ab960fc61f | 149 | vs = compassPID; |
akudohune | 0:13ab960fc61f | 150 | |
akudohune | 0:13ab960fc61f | 151 | if(IR_found){ |
akudohune | 0:13ab960fc61f | 152 | if((direction == 4) || (direction == 12)||(direction == 3) || (direction == 5) || (direction ==11) || (direction == 13)){ |
akudohune | 0:13ab960fc61f | 153 | vx = (int)(20*ball_sankaku[direction][0]); |
akudohune | 0:13ab960fc61f | 154 | vy = (int)(20*ball_sankaku[direction][1]); |
akudohune | 0:13ab960fc61f | 155 | }else{ |
akudohune | 0:13ab960fc61f | 156 | vx = (int)(10*ball_sankaku[direction][0]); |
akudohune | 0:13ab960fc61f | 157 | vy = (int)(10*ball_sankaku[direction][1]); |
akudohune | 0:13ab960fc61f | 158 | } |
akudohune | 0:13ab960fc61f | 159 | |
akudohune | 0:13ab960fc61f | 160 | if(Distance <= 10){ |
akudohune | 0:13ab960fc61f | 161 | vx *= -1; |
akudohune | 0:13ab960fc61f | 162 | vy *= -1; |
akudohune | 0:13ab960fc61f | 163 | } |
akudohune | 0:13ab960fc61f | 164 | }else{ |
akudohune | 0:13ab960fc61f | 165 | vx = 0; |
akudohune | 0:13ab960fc61f | 166 | vy = 0; |
akudohune | 0:13ab960fc61f | 167 | } |
akudohune | 0:13ab960fc61f | 168 | |
akudohune | 0:13ab960fc61f | 169 | |
akudohune | 0:13ab960fc61f | 170 | move(vx,vy,vs); |
akudohune | 0:13ab960fc61f | 171 | } |
akudohune | 0:13ab960fc61f | 172 | } |