Ryo Ogata / Mbed 2 deprecated walkROBO

歩行ロボ

Dependencies:   HMC6352 QEI Servo mbed

main.cpp@1:f465d89a26b0, 2013-08-01 (annotated)

Committer:
OGA
Date:
Thu Aug 01 09:05:23 2013 +0000
Revision:
1:f465d89a26b0
Parent:
0:4644bf6bca6a 
????

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);
OGA 1:f465d89a26b0 11 HMC6352 compass(p28/*sda*/, p27/*scl*/);
OGA 0:4644bf6bca6a 12 Servo myservo1(p21);
OGA 0:4644bf6bca6a 13 Servo myservo2(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 }*/
OGA 0:4644bf6bca6a 64
OGA 0:4644bf6bca6a 65
OGA 0:4644bf6bca6a 66
OGA 0:4644bf6bca6a 67 int main() {
OGA 0:4644bf6bca6a 68
OGA 0:4644bf6bca6a 69
OGA 0:4644bf6bca6a 70
OGA 0:4644bf6bca6a 71
OGA 0:4644bf6bca6a 72 //printf("test\n");
OGA 0:4644bf6bca6a 73
OGA 0:4644bf6bca6a 74 timer2.start();
OGA 0:4644bf6bca6a 75 interrupt.attach(&tic_sensor, 0.1/*sec*/);
OGA 1:f465d89a26b0 76 compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
OGA 0:4644bf6bca6a 77 //printf("test%d\n",com_val);
OGA 0:4644bf6bca6a 78
OGA 0:4644bf6bca6a 79 while(1) {
OGA 0:4644bf6bca6a 80 wait(0.1);
OGA 0:4644bf6bca6a 81
OGA 0:4644bf6bca6a 82
OGA 1:f465d89a26b0 83 // printf("pid:%.5d\n", ultrasonicVal[0]);
OGA 1:f465d89a26b0 84 printf("Heading is: %f\n", compass.sample() / 10.0);
OGA 1:f465d89a26b0 85
OGA 0:4644bf6bca6a 86 if((ultrasonicVal[0] < 1000)&&(ultrasonicVal[1] < 1000)){
OGA 0:4644bf6bca6a 87 myservo1 = 0.5;
OGA 0:4644bf6bca6a 88 myservo2 = 0.5;
OGA 0:4644bf6bca6a 89 }else if((ultrasonicVal[0] < ultrasonicVal[1])&&(ultrasonicVal[0] < 1000)){
OGA 0:4644bf6bca6a 90 myservo1 = 0.7 + proportional;
OGA 0:4644bf6bca6a 91 myservo2 = 0.7;
OGA 0:4644bf6bca6a 92 }else if((ultrasonicVal[0] > ultrasonicVal[1])&&(ultrasonicVal[1] < 1000)){
OGA 0:4644bf6bca6a 93 myservo1 = 0.3;
OGA 0:4644bf6bca6a 94 myservo2 = 0.3 + proportional;
OGA 0:4644bf6bca6a 95 }
OGA 0:4644bf6bca6a 96 }
OGA 0:4644bf6bca6a 97 }