aa
Dependencies: HMC6352 QEI Servo mbed
Fork of walkROBO by
main.cpp@2:955cdadf5ecc, 2013-09-05 (annotated)
- Committer:
- yusuke_robocup
- Date:
- Thu Sep 05 09:57:22 2013 +0000
- Revision:
- 2:955cdadf5ecc
- Parent:
- 1:f465d89a26b0
aa
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OGA | 0:4644bf6bca6a | 1 | #include "mbed.h" |
OGA | 0:4644bf6bca6a | 2 | //#include "QEI.h" |
OGA | 0:4644bf6bca6a | 3 | #include "HMC6352.h" |
OGA | 0:4644bf6bca6a | 4 | #include "Servo.h" |
OGA | 0:4644bf6bca6a | 5 | #include "main.h" |
OGA | 0:4644bf6bca6a | 6 | |
OGA | 0:4644bf6bca6a | 7 | #define OUTRANGE_MAX 0.2//pid |
OGA | 0:4644bf6bca6a | 8 | //#define ROTATE_PER_REVOLUTIONS 360//enko-da no bunkainou |
OGA | 0:4644bf6bca6a | 9 | |
OGA | 0:4644bf6bca6a | 10 | //QEI wheel(p23/*A*/, p24/*B*/, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); |
yusuke_robocup | 2:955cdadf5ecc | 11 | //HMC6352 compass(p28/*sda*/, p27/*scl*/); |
yusuke_robocup | 2:955cdadf5ecc | 12 | Servo servoR(p21); |
yusuke_robocup | 2:955cdadf5ecc | 13 | Servo servoL(p22); |
OGA | 0:4644bf6bca6a | 14 | DigitalOut myled(LED1); |
OGA | 0:4644bf6bca6a | 15 | Ticker interrupt; |
OGA | 0:4644bf6bca6a | 16 | |
OGA | 0:4644bf6bca6a | 17 | double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD); |
OGA | 0:4644bf6bca6a | 18 | |
OGA | 0:4644bf6bca6a | 19 | double proportional = 0; |
OGA | 0:4644bf6bca6a | 20 | uint16_t com_val = 0; |
OGA | 0:4644bf6bca6a | 21 | |
OGA | 0:4644bf6bca6a | 22 | |
OGA | 0:4644bf6bca6a | 23 | void tic_sensor() |
OGA | 0:4644bf6bca6a | 24 | { |
OGA | 0:4644bf6bca6a | 25 | //int temp = (double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4); |
OGA | 0:4644bf6bca6a | 26 | Ultrasonic(); |
OGA | 0:4644bf6bca6a | 27 | /*com_val = (compass.sample() / 10 + 180)%360;//180°を中間値にする |
OGA | 0:4644bf6bca6a | 28 | //proportional = PIDRead(com_val, 180, 0.002, 0, 0); |
OGA | 0:4644bf6bca6a | 29 | |
OGA | 0:4644bf6bca6a | 30 | proportional = 0.002*(com_val-180); |
OGA | 0:4644bf6bca6a | 31 | |
OGA | 0:4644bf6bca6a | 32 | if(proportional > OUTRANGE_MAX){ |
OGA | 0:4644bf6bca6a | 33 | proportional = OUTRANGE_MAX; |
OGA | 0:4644bf6bca6a | 34 | }else if(proportional < -OUTRANGE_MAX){ |
OGA | 0:4644bf6bca6a | 35 | proportional = -OUTRANGE_MAX; |
OGA | 0:4644bf6bca6a | 36 | }*/ |
OGA | 0:4644bf6bca6a | 37 | } |
OGA | 0:4644bf6bca6a | 38 | |
OGA | 0:4644bf6bca6a | 39 | |
OGA | 0:4644bf6bca6a | 40 | /*double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD) |
OGA | 0:4644bf6bca6a | 41 | { |
OGA | 0:4644bf6bca6a | 42 | static double diff[2] = {0}; |
OGA | 0:4644bf6bca6a | 43 | double p,i,d = 0; |
OGA | 0:4644bf6bca6a | 44 | double m; |
OGA | 0:4644bf6bca6a | 45 | static int16_t integral = 0; |
OGA | 0:4644bf6bca6a | 46 | |
OGA | 0:4644bf6bca6a | 47 | diff[0] = diff[1]; |
OGA | 0:4644bf6bca6a | 48 | diff[1] = (double)(sensor - target); |
OGA | 0:4644bf6bca6a | 49 | integral += (diff[0] + diff[1])/2; |
OGA | 0:4644bf6bca6a | 50 | |
OGA | 0:4644bf6bca6a | 51 | p = KP * diff[1]; |
OGA | 0:4644bf6bca6a | 52 | i = KI * integral; |
OGA | 0:4644bf6bca6a | 53 | d = KD * (diff[1] - diff[0]); |
OGA | 0:4644bf6bca6a | 54 | m = p+i+d; |
OGA | 0:4644bf6bca6a | 55 | |
OGA | 0:4644bf6bca6a | 56 | if(m > OUTRANGE_MAX){ |
OGA | 0:4644bf6bca6a | 57 | m = OUTRANGE_MAX; |
OGA | 0:4644bf6bca6a | 58 | }else if(m < -OUTRANGE_MAX){ |
OGA | 0:4644bf6bca6a | 59 | m = -OUTRANGE_MAX; |
OGA | 0:4644bf6bca6a | 60 | } |
OGA | 0:4644bf6bca6a | 61 | |
OGA | 0:4644bf6bca6a | 62 | return m; |
OGA | 0:4644bf6bca6a | 63 | }*/ |
yusuke_robocup | 2:955cdadf5ecc | 64 | |
yusuke_robocup | 2:955cdadf5ecc | 65 | #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0) |
OGA | 0:4644bf6bca6a | 66 | |
yusuke_robocup | 2:955cdadf5ecc | 67 | #define STRAIGHT 0.6; |
yusuke_robocup | 2:955cdadf5ecc | 68 | #define SPIN 0.4; |
yusuke_robocup | 2:955cdadf5ecc | 69 | |
yusuke_robocup | 2:955cdadf5ecc | 70 | //#define STRAIGHT 0.0; |
yusuke_robocup | 2:955cdadf5ecc | 71 | //#define SPIN 1.0; |
yusuke_robocup | 2:955cdadf5ecc | 72 | |
yusuke_robocup | 2:955cdadf5ecc | 73 | |
yusuke_robocup | 2:955cdadf5ecc | 74 | void move(int vl,int vs){ |
yusuke_robocup | 2:955cdadf5ecc | 75 | double fut_R,fut_L,true_vs; |
yusuke_robocup | 2:955cdadf5ecc | 76 | |
yusuke_robocup | 2:955cdadf5ecc | 77 | true_vs = abs(vs)/SPIN; |
yusuke_robocup | 2:955cdadf5ecc | 78 | |
yusuke_robocup | 2:955cdadf5ecc | 79 | if(true_vs > 40){ |
yusuke_robocup | 2:955cdadf5ecc | 80 | vl = 0; |
yusuke_robocup | 2:955cdadf5ecc | 81 | vs = 100*(vs/abs(vs)); |
yusuke_robocup | 2:955cdadf5ecc | 82 | } |
yusuke_robocup | 2:955cdadf5ecc | 83 | |
yusuke_robocup | 2:955cdadf5ecc | 84 | fut_R = Convert_dekaruto((vl + vs)); |
yusuke_robocup | 2:955cdadf5ecc | 85 | fut_L = Convert_dekaruto(-(vl - vs)*1.4); |
yusuke_robocup | 2:955cdadf5ecc | 86 | |
yusuke_robocup | 2:955cdadf5ecc | 87 | servoR = fut_R; |
yusuke_robocup | 2:955cdadf5ecc | 88 | servoL = fut_L; |
yusuke_robocup | 2:955cdadf5ecc | 89 | |
yusuke_robocup | 2:955cdadf5ecc | 90 | //printf("R:%lf L:%lf\n",fut_R,fut_L); |
yusuke_robocup | 2:955cdadf5ecc | 91 | //printf("R:%d L:%d\n",(vl + vs),-(vl - vs)); |
yusuke_robocup | 2:955cdadf5ecc | 92 | } |
yusuke_robocup | 2:955cdadf5ecc | 93 | |
yusuke_robocup | 2:955cdadf5ecc | 94 | void PidUpdate() |
yusuke_robocup | 2:955cdadf5ecc | 95 | { |
yusuke_robocup | 2:955cdadf5ecc | 96 | inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0); |
yusuke_robocup | 2:955cdadf5ecc | 97 | //printf("%lf\n",inputPID); |
yusuke_robocup | 2:955cdadf5ecc | 98 | } |
yusuke_robocup | 2:955cdadf5ecc | 99 | |
yusuke_robocup | 2:955cdadf5ecc | 100 | double vsOut(){ |
yusuke_robocup | 2:955cdadf5ecc | 101 | double vs; |
yusuke_robocup | 2:955cdadf5ecc | 102 | vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1; |
yusuke_robocup | 2:955cdadf5ecc | 103 | vs = vs * 8; |
yusuke_robocup | 2:955cdadf5ecc | 104 | |
yusuke_robocup | 2:955cdadf5ecc | 105 | if(vs/abs(vs) < 0){ |
yusuke_robocup | 2:955cdadf5ecc | 106 | //vs *= 1.3; |
yusuke_robocup | 2:955cdadf5ecc | 107 | } |
yusuke_robocup | 2:955cdadf5ecc | 108 | |
yusuke_robocup | 2:955cdadf5ecc | 109 | if(abs(vs) > 90)vs = 90*(vs/abs(vs)); |
yusuke_robocup | 2:955cdadf5ecc | 110 | if(abs(vs) < 25) vs = 25*(vs/abs(vs)); |
yusuke_robocup | 2:955cdadf5ecc | 111 | |
yusuke_robocup | 2:955cdadf5ecc | 112 | return vs; |
yusuke_robocup | 2:955cdadf5ecc | 113 | } |
OGA | 0:4644bf6bca6a | 114 | |
OGA | 0:4644bf6bca6a | 115 | |
OGA | 0:4644bf6bca6a | 116 | int main() { |
yusuke_robocup | 2:955cdadf5ecc | 117 | |
yusuke_robocup | 2:955cdadf5ecc | 118 | wait(3); |
yusuke_robocup | 2:955cdadf5ecc | 119 | |
yusuke_robocup | 2:955cdadf5ecc | 120 | int vl; |
yusuke_robocup | 2:955cdadf5ecc | 121 | double vs; |
OGA | 0:4644bf6bca6a | 122 | |
OGA | 0:4644bf6bca6a | 123 | timer2.start(); |
yusuke_robocup | 2:955cdadf5ecc | 124 | //interrupt.attach(&tic_sensor, 0.1/*sec*/); |
OGA | 1:f465d89a26b0 | 125 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
yusuke_robocup | 2:955cdadf5ecc | 126 | pidUpdata.attach(&PidUpdate, PID_CYCLE); |
OGA | 0:4644bf6bca6a | 127 | //printf("test%d\n",com_val); |
OGA | 0:4644bf6bca6a | 128 | |
OGA | 0:4644bf6bca6a | 129 | while(1) { |
yusuke_robocup | 2:955cdadf5ecc | 130 | vl = -90; |
yusuke_robocup | 2:955cdadf5ecc | 131 | //vl = 0; |
yusuke_robocup | 2:955cdadf5ecc | 132 | |
yusuke_robocup | 2:955cdadf5ecc | 133 | vl = vl * STRAIGHT; |
yusuke_robocup | 2:955cdadf5ecc | 134 | vs = vsOut() * SPIN; |
OGA | 0:4644bf6bca6a | 135 | |
yusuke_robocup | 2:955cdadf5ecc | 136 | move(vl,(int)vs); |
OGA | 0:4644bf6bca6a | 137 | } |
OGA | 0:4644bf6bca6a | 138 | } |