aa
Dependencies: HMC6352 QEI Servo mbed
Fork of walkROBO by
main.cpp
- Committer:
- OGA
- Date:
- 2013-07-31
- Revision:
- 0:4644bf6bca6a
- Child:
- 1:f465d89a26b0
File content as of revision 0:4644bf6bca6a:
#include "mbed.h" //#include "QEI.h" #include "HMC6352.h" #include "Servo.h" #include "main.h" #define OUTRANGE_MAX 0.2//pid //#define ROTATE_PER_REVOLUTIONS 360//enko-da no bunkainou //QEI wheel(p23/*A*/, p24/*B*/, NC, ROTATE_PER_REVOLUTIONS, QEI::X4_ENCODING); //HMC6352 compass(p28/*sda*/, p27/*scl*/); Servo myservo1(p21); Servo myservo2(p22); DigitalOut myled(LED1); Ticker interrupt; double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD); double proportional = 0; uint16_t com_val = 0; void tic_sensor() { //int temp = (double)wheel.getPulses()/(ROTATE_PER_REVOLUTIONS*4); Ultrasonic(); /*com_val = (compass.sample() / 10 + 180)%360;//180°を中間値にする //proportional = PIDRead(com_val, 180, 0.002, 0, 0); proportional = 0.002*(com_val-180); if(proportional > OUTRANGE_MAX){ proportional = OUTRANGE_MAX; }else if(proportional < -OUTRANGE_MAX){ proportional = -OUTRANGE_MAX; }*/ } /*double PIDRead(uint8_t sensor, uint8_t target, uint8_t KP, uint8_t KI, uint8_t KD) { static double diff[2] = {0}; double p,i,d = 0; double m; static int16_t integral = 0; diff[0] = diff[1]; diff[1] = (double)(sensor - target); integral += (diff[0] + diff[1])/2; p = KP * diff[1]; i = KI * integral; d = KD * (diff[1] - diff[0]); m = p+i+d; if(m > OUTRANGE_MAX){ m = OUTRANGE_MAX; }else if(m < -OUTRANGE_MAX){ m = -OUTRANGE_MAX; } return m; }*/ int main() { //printf("test\n"); timer2.start(); interrupt.attach(&tic_sensor, 0.1/*sec*/); //compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); //printf("test%d\n",com_val); while(1) { wait(0.1); printf("pid:%.5d\n", ultrasonicVal[0]); if((ultrasonicVal[0] < 1000)&&(ultrasonicVal[1] < 1000)){ myservo1 = 0.5; myservo2 = 0.5; }else if((ultrasonicVal[0] < ultrasonicVal[1])&&(ultrasonicVal[0] < 1000)){ myservo1 = 0.7 + proportional; myservo2 = 0.7; }else if((ultrasonicVal[0] > ultrasonicVal[1])&&(ultrasonicVal[1] < 1000)){ myservo1 = 0.3; myservo2 = 0.3 + proportional; } } }