yusuke takahashi / Mbed 2 deprecated walkROBO_3

aa

Dependencies:   HMC6352 QEI Servo mbed

Fork of walkROBO by Ryo Ogata

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?

UserRevisionLine numberNew 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 }