yusuke takahashi / Mbed 2 deprecated PIDsync_0_2_2

aaaaa

Dependencies:   HMC6352 PID QEI Servo mbed

main.cpp@0:1be472d79ae9, 2013-09-30 (annotated)

Committer:
yusuke_robocup
Date:
Mon Sep 30 09:01:37 2013 +0000
Revision:
0:1be472d79ae9
PIDsync

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yusuke_robocup 0:1be472d79ae9 1 #include "mbed.h"
yusuke_robocup 0:1be472d79ae9 2 #include "PID.h"
yusuke_robocup 0:1be472d79ae9 3 #include "QEI.h"
yusuke_robocup 0:1be472d79ae9 4 #include "Servo.h"
yusuke_robocup 0:1be472d79ae9 5 #include "HMC6352.h"
yusuke_robocup 0:1be472d79ae9 6 #include "main.h"
yusuke_robocup 0:1be472d79ae9 7
yusuke_robocup 0:1be472d79ae9 8 //guro-baru
yusuke_robocup 0:1be472d79ae9 9 double vl = 0;
yusuke_robocup 0:1be472d79ae9 10 double vc = 0;
yusuke_robocup 0:1be472d79ae9 11 double vs = 1;
yusuke_robocup 0:1be472d79ae9 12 int clwise = 0;
yusuke_robocup 0:1be472d79ae9 13 int anclwise = 0;
yusuke_robocup 0:1be472d79ae9 14
yusuke_robocup 0:1be472d79ae9 15 int enkoda1;
yusuke_robocup 0:1be472d79ae9 16 int enkoda2;
yusuke_robocup 0:1be472d79ae9 17
yusuke_robocup 0:1be472d79ae9 18 int process = 0;
yusuke_robocup 0:1be472d79ae9 19 int vc_count = 0;
yusuke_robocup 0:1be472d79ae9 20 int mode_comp = 0;
yusuke_robocup 0:1be472d79ae9 21
yusuke_robocup 0:1be472d79ae9 22 int inputPID = 0;
yusuke_robocup 0:1be472d79ae9 23
yusuke_robocup 0:1be472d79ae9 24 //guro-baru end
yusuke_robocup 0:1be472d79ae9 25
yusuke_robocup 0:1be472d79ae9 26
yusuke_robocup 0:1be472d79ae9 27 void PidUpdate(){
yusuke_robocup 0:1be472d79ae9 28 static int state = WAIT,past_state = WAIT,next_state = WAIT;
yusuke_robocup 0:1be472d79ae9 29
yusuke_robocup 0:1be472d79ae9 30 if(!mode_comp){
yusuke_robocup 0:1be472d79ae9 31 if(vl && !vs){
yusuke_robocup 0:1be472d79ae9 32 state = STRAIGHT;
yusuke_robocup 0:1be472d79ae9 33 }
yusuke_robocup 0:1be472d79ae9 34 if(vs){
yusuke_robocup 0:1be472d79ae9 35 state = TURN;
yusuke_robocup 0:1be472d79ae9 36 }
yusuke_robocup 0:1be472d79ae9 37 }
yusuke_robocup 0:1be472d79ae9 38 if((state != past_state)){
yusuke_robocup 0:1be472d79ae9 39 mode_comp = 1;
yusuke_robocup 0:1be472d79ae9 40
yusuke_robocup 0:1be472d79ae9 41 if(process == 0){
yusuke_robocup 0:1be472d79ae9 42 if(abs(enkoda1) > 180 && abs(enkoda1) < 240){
yusuke_robocup 0:1be472d79ae9 43 process++;
yusuke_robocup 0:1be472d79ae9 44 }
yusuke_robocup 0:1be472d79ae9 45 }
yusuke_robocup 0:1be472d79ae9 46 if(process == 1){
yusuke_robocup 0:1be472d79ae9 47 if(state == STRAIGHT){
yusuke_robocup 0:1be472d79ae9 48 vl = 10;
yusuke_robocup 0:1be472d79ae9 49 vs = 0;
yusuke_robocup 0:1be472d79ae9 50 }
yusuke_robocup 0:1be472d79ae9 51 if(state == TURN){
yusuke_robocup 0:1be472d79ae9 52 vl = 0;
yusuke_robocup 0:1be472d79ae9 53 vs = 10;
yusuke_robocup 0:1be472d79ae9 54 }
yusuke_robocup 0:1be472d79ae9 55 if(abs(vc) < 60){
yusuke_robocup 0:1be472d79ae9 56 vc_count++;
yusuke_robocup 0:1be472d79ae9 57 }
yusuke_robocup 0:1be472d79ae9 58 if(vc_count > 2){
yusuke_robocup 0:1be472d79ae9 59 vc_count = 0;
yusuke_robocup 0:1be472d79ae9 60 mode_comp = 0;
yusuke_robocup 0:1be472d79ae9 61 process = 0;
yusuke_robocup 0:1be472d79ae9 62 }
yusuke_robocup 0:1be472d79ae9 63 }
yusuke_robocup 0:1be472d79ae9 64 }
yusuke_robocup 0:1be472d79ae9 65
yusuke_robocup 0:1be472d79ae9 66 enkoda1 = (int)(((double)wheel1.getPulses()/CYCLE) * 360.0);
yusuke_robocup 0:1be472d79ae9 67 enkoda2 = (int)(((double)wheel2.getPulses()/CYCLE) * 360.0);
yusuke_robocup 0:1be472d79ae9 68
yusuke_robocup 0:1be472d79ae9 69 if((!vs)&&(mode_comp == 0)){
yusuke_robocup 0:1be472d79ae9 70 pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
yusuke_robocup 0:1be472d79ae9 71 }else if((vs)&&(mode_comp == 0)){
yusuke_robocup 0:1be472d79ae9 72 pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
yusuke_robocup 0:1be472d79ae9 73 }
yusuke_robocup 0:1be472d79ae9 74
yusuke_robocup 0:1be472d79ae9 75 if((!vs)&&(mode_comp)){
yusuke_robocup 0:1be472d79ae9 76 pid.setProcessValue(wheel1.getPulses() + wheel2.getPulses());
yusuke_robocup 0:1be472d79ae9 77 }else if((vs)&&(mode_comp)){
yusuke_robocup 0:1be472d79ae9 78 pid.setProcessValue(wheel1.getPulses() - wheel2.getPulses());
yusuke_robocup 0:1be472d79ae9 79 }
yusuke_robocup 0:1be472d79ae9 80
yusuke_robocup 0:1be472d79ae9 81 vc = (int)pid.compute();
yusuke_robocup 0:1be472d79ae9 82
yusuke_robocup 0:1be472d79ae9 83 printf("vc:%lf\n",vc);
yusuke_robocup 0:1be472d79ae9 84
yusuke_robocup 0:1be472d79ae9 85 double fut_R = 0.5,fut_L = 0.5;
yusuke_robocup 0:1be472d79ae9 86
yusuke_robocup 0:1be472d79ae9 87 if(abs(vc) > 250){
yusuke_robocup 0:1be472d79ae9 88 vc = 0;
yusuke_robocup 0:1be472d79ae9 89 }
yusuke_robocup 0:1be472d79ae9 90
yusuke_robocup 0:1be472d79ae9 91 int vlR = 0;
yusuke_robocup 0:1be472d79ae9 92 int vlL = 0;
yusuke_robocup 0:1be472d79ae9 93 int vcR = vc;
yusuke_robocup 0:1be472d79ae9 94 int vcL = vc;
yusuke_robocup 0:1be472d79ae9 95
yusuke_robocup 0:1be472d79ae9 96 vlR = -vs;
yusuke_robocup 0:1be472d79ae9 97 vlL = vs;
yusuke_robocup 0:1be472d79ae9 98
yusuke_robocup 0:1be472d79ae9 99 vl *= 0.5;
yusuke_robocup 0:1be472d79ae9 100 vc *= 0.5;
yusuke_robocup 0:1be472d79ae9 101
yusuke_robocup 0:1be472d79ae9 102 vlR *= 0.4;
yusuke_robocup 0:1be472d79ae9 103 vlL *= 0.4;
yusuke_robocup 0:1be472d79ae9 104
yusuke_robocup 0:1be472d79ae9 105 vcR *= 0.6;
yusuke_robocup 0:1be472d79ae9 106 vcL *= 0.6;
yusuke_robocup 0:1be472d79ae9 107
yusuke_robocup 0:1be472d79ae9 108
yusuke_robocup 0:1be472d79ae9 109 if(!vs){
yusuke_robocup 0:1be472d79ae9 110 if(vl > 0){
yusuke_robocup 0:1be472d79ae9 111 fut_R = Convert_dekaruto((int)(vl - vc));
yusuke_robocup 0:1be472d79ae9 112 fut_L = Convert_dekaruto((int)(vl * 0.95 + vc));
yusuke_robocup 0:1be472d79ae9 113 }
yusuke_robocup 0:1be472d79ae9 114 if(vl < 0){
yusuke_robocup 0:1be472d79ae9 115 vc *= -1;
yusuke_robocup 0:1be472d79ae9 116 fut_R = Convert_dekaruto((int)(vl * 0.95 + vc));
yusuke_robocup 0:1be472d79ae9 117 fut_L = Convert_dekaruto((int)(vl - vc));
yusuke_robocup 0:1be472d79ae9 118 }
yusuke_robocup 0:1be472d79ae9 119 }else{
yusuke_robocup 0:1be472d79ae9 120 if(vlR < 0){
yusuke_robocup 0:1be472d79ae9 121 vcR *= -1;
yusuke_robocup 0:1be472d79ae9 122 }
yusuke_robocup 0:1be472d79ae9 123
yusuke_robocup 0:1be472d79ae9 124 if(vlL < 0){
yusuke_robocup 0:1be472d79ae9 125 vcL *= -1;
yusuke_robocup 0:1be472d79ae9 126 }
yusuke_robocup 0:1be472d79ae9 127 if(vs > 0){
yusuke_robocup 0:1be472d79ae9 128 fut_R = Convert_dekaruto((int)(vlR * 0.9 + vcR));
yusuke_robocup 0:1be472d79ae9 129 fut_L = Convert_dekaruto((int)(vlL * 0.65 - vcL));
yusuke_robocup 0:1be472d79ae9 130 }
yusuke_robocup 0:1be472d79ae9 131
yusuke_robocup 0:1be472d79ae9 132 if(vs < 0){
yusuke_robocup 0:1be472d79ae9 133 fut_R = Convert_dekaruto((int)(vlR * 0.65 - vcR));
yusuke_robocup 0:1be472d79ae9 134 fut_L = Convert_dekaruto((int)(vlL * 0.9+ vcL));
yusuke_robocup 0:1be472d79ae9 135 }
yusuke_robocup 0:1be472d79ae9 136 }
yusuke_robocup 0:1be472d79ae9 137
yusuke_robocup 0:1be472d79ae9 138 servoR = fut_R;
yusuke_robocup 0:1be472d79ae9 139 servoL = fut_L;
yusuke_robocup 0:1be472d79ae9 140
yusuke_robocup 0:1be472d79ae9 141 if(!mode_comp){
yusuke_robocup 0:1be472d79ae9 142 past_state = state;
yusuke_robocup 0:1be472d79ae9 143 }
yusuke_robocup 0:1be472d79ae9 144 }
yusuke_robocup 0:1be472d79ae9 145
yusuke_robocup 0:1be472d79ae9 146 void CompassUpdate(){
yusuke_robocup 0:1be472d79ae9 147 inputPID = (((int)(compass.sample() - (/**/180 * 10.0) + 5400.0) % 3600) / 10.0);
yusuke_robocup 0:1be472d79ae9 148 }
yusuke_robocup 0:1be472d79ae9 149
yusuke_robocup 0:1be472d79ae9 150 double vssOut(){
yusuke_robocup 0:1be472d79ae9 151 double vss;
yusuke_robocup 0:1be472d79ae9 152 vss = ((inputPID / 360.0 - 0.5) * 2.0 * 1000.0) * 1.0;
yusuke_robocup 0:1be472d79ae9 153
yusuke_robocup 0:1be472d79ae9 154 if(abs(vss) < 10){
yusuke_robocup 0:1be472d79ae9 155 vss = 0;
yusuke_robocup 0:1be472d79ae9 156 }
yusuke_robocup 0:1be472d79ae9 157
yusuke_robocup 0:1be472d79ae9 158 vss *= 3.0;
yusuke_robocup 0:1be472d79ae9 159
yusuke_robocup 0:1be472d79ae9 160
yusuke_robocup 0:1be472d79ae9 161 if((vss)&&(abs(vss) < 500)){
yusuke_robocup 0:1be472d79ae9 162 vss += (vss/abs(vss)) * 500;
yusuke_robocup 0:1be472d79ae9 163 }
yusuke_robocup 0:1be472d79ae9 164
yusuke_robocup 0:1be472d79ae9 165 if(abs(vss) > 1000){
yusuke_robocup 0:1be472d79ae9 166 vss = 1000 * (vss/abs(vss));
yusuke_robocup 0:1be472d79ae9 167 }
yusuke_robocup 0:1be472d79ae9 168
yusuke_robocup 0:1be472d79ae9 169 return vss;
yusuke_robocup 0:1be472d79ae9 170 }
yusuke_robocup 0:1be472d79ae9 171
yusuke_robocup 0:1be472d79ae9 172 void move(int vll,int vss){
yusuke_robocup 0:1be472d79ae9 173 if(!swR){
yusuke_robocup 0:1be472d79ae9 174 wheel1.reset();
yusuke_robocup 0:1be472d79ae9 175 }
yusuke_robocup 0:1be472d79ae9 176
yusuke_robocup 0:1be472d79ae9 177 if(!swL){
yusuke_robocup 0:1be472d79ae9 178 wheel2.reset();
yusuke_robocup 0:1be472d79ae9 179 }
yusuke_robocup 0:1be472d79ae9 180
yusuke_robocup 0:1be472d79ae9 181 vl = vll;
yusuke_robocup 0:1be472d79ae9 182 vs = vss;
yusuke_robocup 0:1be472d79ae9 183 }
yusuke_robocup 0:1be472d79ae9 184
yusuke_robocup 0:1be472d79ae9 185 #define PINR_THR 2000
yusuke_robocup 0:1be472d79ae9 186
yusuke_robocup 0:1be472d79ae9 187 int ping_button(int ping,int button){
yusuke_robocup 0:1be472d79ae9 188 static int continue_flag = 0;
yusuke_robocup 0:1be472d79ae9 189 static int change_flag = 0;
yusuke_robocup 0:1be472d79ae9 190
yusuke_robocup 0:1be472d79ae9 191 if(continue_flag == 0){
yusuke_robocup 0:1be472d79ae9 192 if(ping <= PINR_THR){
yusuke_robocup 0:1be472d79ae9 193 ping_t.start();
yusuke_robocup 0:1be472d79ae9 194 continue_flag = 1;
yusuke_robocup 0:1be472d79ae9 195 }
yusuke_robocup 0:1be472d79ae9 196 }
yusuke_robocup 0:1be472d79ae9 197
yusuke_robocup 0:1be472d79ae9 198 if(continue_flag == 1){
yusuke_robocup 0:1be472d79ae9 199 //agatterutoki
yusuke_robocup 0:1be472d79ae9 200 if(ping <= PINR_THR){
yusuke_robocup 0:1be472d79ae9 201 if(change_flag == 0){
yusuke_robocup 0:1be472d79ae9 202 if(ping_t.read_ms() >= 150){
yusuke_robocup 0:1be472d79ae9 203 button = !button;
yusuke_robocup 0:1be472d79ae9 204 change_flag = 1;
yusuke_robocup 0:1be472d79ae9 205 }
yusuke_robocup 0:1be472d79ae9 206 }
yusuke_robocup 0:1be472d79ae9 207 }
yusuke_robocup 0:1be472d79ae9 208 //tatisagari
yusuke_robocup 0:1be472d79ae9 209 if(ping >= (PINR_THR+200)){
yusuke_robocup 0:1be472d79ae9 210 ping_t.stop();
yusuke_robocup 0:1be472d79ae9 211 ping_t.reset();
yusuke_robocup 0:1be472d79ae9 212 continue_flag = 0;
yusuke_robocup 0:1be472d79ae9 213 change_flag = 0;
yusuke_robocup 0:1be472d79ae9 214 }
yusuke_robocup 0:1be472d79ae9 215 }
yusuke_robocup 0:1be472d79ae9 216 return button;
yusuke_robocup 0:1be472d79ae9 217 }
yusuke_robocup 0:1be472d79ae9 218
yusuke_robocup 0:1be472d79ae9 219
yusuke_robocup 0:1be472d79ae9 220 int main() {
yusuke_robocup 0:1be472d79ae9 221 wait(3);
yusuke_robocup 0:1be472d79ae9 222
yusuke_robocup 0:1be472d79ae9 223 timer2.start();
yusuke_robocup 0:1be472d79ae9 224 ping_t.start();
yusuke_robocup 0:1be472d79ae9 225
yusuke_robocup 0:1be472d79ae9 226 pid.setInputLimits(MINIMUM,MAXIMUM);
yusuke_robocup 0:1be472d79ae9 227 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT);
yusuke_robocup 0:1be472d79ae9 228 pid.setBias(PID_BIAS);
yusuke_robocup 0:1be472d79ae9 229 pid.setMode(AUTO_MODE);
yusuke_robocup 0:1be472d79ae9 230 pid.setSetPoint(REFERENCE);
yusuke_robocup 0:1be472d79ae9 231
yusuke_robocup 0:1be472d79ae9 232 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
yusuke_robocup 0:1be472d79ae9 233 compassUpdata.attach(&CompassUpdate, COMPASS_CYCLE);
yusuke_robocup 0:1be472d79ae9 234
yusuke_robocup 0:1be472d79ae9 235 swR.mode(PullUp);
yusuke_robocup 0:1be472d79ae9 236 swL.mode(PullUp);
yusuke_robocup 0:1be472d79ae9 237
yusuke_robocup 0:1be472d79ae9 238 int setR = 0,setL = 0;
yusuke_robocup 0:1be472d79ae9 239 int vll = 0,vss = 0;
yusuke_robocup 0:1be472d79ae9 240
yusuke_robocup 0:1be472d79ae9 241 servoR = 0.595;
yusuke_robocup 0:1be472d79ae9 242 servoL = 0.59;
yusuke_robocup 0:1be472d79ae9 243
yusuke_robocup 0:1be472d79ae9 244 while(1){
yusuke_robocup 0:1be472d79ae9 245 if(!swR){
yusuke_robocup 0:1be472d79ae9 246 setR = 1;
yusuke_robocup 0:1be472d79ae9 247 }
yusuke_robocup 0:1be472d79ae9 248
yusuke_robocup 0:1be472d79ae9 249 if(!swL){
yusuke_robocup 0:1be472d79ae9 250 setL = 1;
yusuke_robocup 0:1be472d79ae9 251 }
yusuke_robocup 0:1be472d79ae9 252
yusuke_robocup 0:1be472d79ae9 253 if(setR){
yusuke_robocup 0:1be472d79ae9 254 servoR = 0.5;
yusuke_robocup 0:1be472d79ae9 255 }
yusuke_robocup 0:1be472d79ae9 256
yusuke_robocup 0:1be472d79ae9 257 if(setL){
yusuke_robocup 0:1be472d79ae9 258 servoL = 0.5;
yusuke_robocup 0:1be472d79ae9 259 }
yusuke_robocup 0:1be472d79ae9 260
yusuke_robocup 0:1be472d79ae9 261 if(setR && setL){
yusuke_robocup 0:1be472d79ae9 262 break;
yusuke_robocup 0:1be472d79ae9 263 }
yusuke_robocup 0:1be472d79ae9 264
yusuke_robocup 0:1be472d79ae9 265 wait(0.1);
yusuke_robocup 0:1be472d79ae9 266 }
yusuke_robocup 0:1be472d79ae9 267
yusuke_robocup 0:1be472d79ae9 268 wheel1.reset();
yusuke_robocup 0:1be472d79ae9 269 wheel2.reset();
yusuke_robocup 0:1be472d79ae9 270
yusuke_robocup 0:1be472d79ae9 271 pidUpdata.attach(&PidUpdate, PID_CYCLE);
yusuke_robocup 0:1be472d79ae9 272
yusuke_robocup 0:1be472d79ae9 273 int button = 0;
yusuke_robocup 0:1be472d79ae9 274
yusuke_robocup 0:1be472d79ae9 275 while(1) {
yusuke_robocup 0:1be472d79ae9 276 vll = 0;
yusuke_robocup 0:1be472d79ae9 277 vss = 0;
yusuke_robocup 0:1be472d79ae9 278
yusuke_robocup 0:1be472d79ae9 279 Ultrasonic();
yusuke_robocup 0:1be472d79ae9 280
yusuke_robocup 0:1be472d79ae9 281 button = ping_button(ultrasonicVal[0],button);
yusuke_robocup 0:1be472d79ae9 282
yusuke_robocup 0:1be472d79ae9 283 if(button){
yusuke_robocup 0:1be472d79ae9 284 vll = 400;
yusuke_robocup 0:1be472d79ae9 285 led1 = 1;
yusuke_robocup 0:1be472d79ae9 286 }else{
yusuke_robocup 0:1be472d79ae9 287 vll = -500;
yusuke_robocup 0:1be472d79ae9 288 led1 = 0;
yusuke_robocup 0:1be472d79ae9 289 }
yusuke_robocup 0:1be472d79ae9 290
yusuke_robocup 0:1be472d79ae9 291 move(vll,vss);
yusuke_robocup 0:1be472d79ae9 292 }
yusuke_robocup 0:1be472d79ae9 293 }