太郎 九大
/
2013naturobo
夏ロボ用
Diff: init.cpp
- Revision:
- 0:7d18a91d0d19
diff -r 000000000000 -r 7d18a91d0d19 init.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/init.cpp Sat Sep 07 06:34:29 2013 +0000 @@ -0,0 +1,77 @@ +#include "init.h" +#include "QEI.h" + + +DigitalOut MOTOR_POWER(PTC10); + +PwmOut PWM0(PTC9); +PwmOut PWM1(PTC8); +PwmOut PWM2(PTA5); +PwmOut PWM3(PTA4); +PwmOut PWM4(PTA12); +PwmOut PWM5(PTD4); +AnalogIn AD0(PTB0); +AnalogIn AD1(PTB1); +AnalogIn AD2(PTB2); +AnalogIn AD3(PTB3); +AnalogIn AD4(PTC2); + +DigitalOut OUT0(PTC6); +//DigitalIn IN0(PTC6); +DigitalOut OUT1(PTC5); +//DigitalIn IN1(PTC5); +DigitalOut OUT2(PTC4); +//DigitalIn IN2(PTC4); +DigitalOut OUT3(PTC3); +//DigitalIn IN3(PTC3); +DigitalOut OUT4(PTC0); +//DigitalIn IN4(PTC0); +DigitalOut OUT5(PTC7); +//DigitalIn IN5(PTC7); + +Serial pc(USBTX, USBRX); + +QEI ENC0(PTD6, PTD7, NC, 624, QEI::X2_ENCODING); +QEI ENC1(PTD3, PTD1, NC, 624, QEI::X2_ENCODING); +QEI ENC2(PTA13, PTD5, NC, 624, QEI::X2_ENCODING); +QEI ENC3(PTD0, PTD2, NC, 624, QEI::X2_ENCODING); + +I2C i2c0(PTE0,PTE1); + +DigitalIn SW0(PTE29); +DigitalIn SW1(PTE30); + +DigitalOut rled(LED_RED); +DigitalOut gled(LED_GREEN); +DigitalOut bled(LED_BLUE); + +void InitBoard(){ + SW0.mode(PullUp); + SW1.mode(PullUp); + i2c0.frequency(1000000); + MOTOR_POWER = 1; +} + +Timer AdCycle; + +void AdjustCycle(int t_us){ + if(AdCycle.read_us() == 0) AdCycle.start(); + + if(AdCycle.read_us()>t_us)gled=0; + else gled=1; + while(AdCycle.read_us()<=t_us); + AdCycle.reset(); +} + +int Limit(int value,int max,int min) { + if (value>max)return max; + if (value<min)return min; + else return value; +} + +double Limit_d(double value,double max,double min) { + if (value>max)return max; + if (value<min)return min; + else return value; +} +