first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of cool_step by
Diff: main.cpp
- Revision:
- 10:09dbd00164b9
- Parent:
- 9:ca52462f9ebc
- Child:
- 11:ba1a630e56c4
--- a/main.cpp Tue May 05 09:07:11 2015 +0000 +++ b/main.cpp Wed May 13 11:55:50 2015 +0000 @@ -1,11 +1,24 @@ #include "mbed.h" #include "MPU9150.h" #include "Quaternion.h" +#include "Motor.h" + + +void InitTimer0(void); //led pins DigitalOut led1(P2_6); DigitalOut led2(P2_7); DigitalOut led3(P2_8); +AnalogIn hallSensor(P0_26); + +volatile int pulses_ = 0; +InterruptIn hall1_(P0_5), hall2_(P0_4); +//Hallsensor hall1(P1_26, P1_27); +//Hallsensor hall2(P0_4, P0_5); + +Motor motor1(P2_0, P2_1); +//Motor motor2(P2_2, P2_3, &pulses_); MPU9150 imu(P0_28, P0_27, P2_13); @@ -26,15 +39,34 @@ { led1=!led1; } - - +void deneme(void) +{ + led3=!led3; +} int main() { + //Workout what the current state is. + int h1 = hall1_.read(); + int h2 = hall2_.read(); + // int h3 = hall3_.read(); + + // Set the PullUp Resistor + hall1_.mode(PullUp); + hall2_.mode(PullUp); + //hall3_.mode(PullUp); + + //set interrupts on only hall 1-3 rise and fall. + hall1_.rise(&deneme); + hall1_.fall(&deneme); + hall2_.rise(&deneme); + hall2_.fall(&deneme); + //hall3_.rise(this, &deneme); + // hall3_.fall(this, &deneme); RN42.baud(9600); debug.baud(115200); + // InitTimer0(); + // initialize_connection(); - // initialize_connection(); - infoTicker.attach(infoTask,0.2f); if(imu.isReady()) { @@ -57,7 +89,7 @@ if(imu.getFifoCount() >= 48) { imu.getFifoBuffer(buffer, 48); led2 = !led2; - debug.printf("%d\r\n",timer2.read_ms()); + //debug.printf("%d\r\n",timer2.read_ms()); timer2.reset(); } @@ -96,32 +128,106 @@ */ q1.decode(buffer); + + debug.printf("%f, ",hallSensor.read()); debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z); - + Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1); Vector3 pry=q1.getEulerAngles(); - /*Vector3 vector_x_local=q1.rotate(vector_x); - Vector3 vector_y_local=q1.rotate(vector_y); - Vector3 vector_z_local=q1.rotate(vector_z); - Vector3 direction_plane_normal=vector_z_local.cross_product(vector_z); - Vector3 bar_projection=vector_z-vector_x_local*(vector_z.dot_product(vector_x_local)/vector_x_local.length_squared()); - Vector3 bar_projection_g=bar_projection-vector_z*(bar_projection.dot_product(vector_z)/vector_z.length_squared()); - float angle=acos(vector_y_local.dot_product(vector_z)/vector_y_local.length()/vector_z.length());*/ - //debug.printf("angle: %f ", angle/PI*180); - debug.printf("p: %f r: %f y: %f \r\n ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180); + debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180); + /* debug.printf("m1: %d m2: %d \r\n ",motor1.getPulses(),motor2.getPulses()); + motor1.setVelocity(pry.x/90); + motor2.setVelocity(pry.x/90);*/ - //TeaPot Demo Packet for MotionFit SDK - /* - debug.putc('$'); //packet start - debug.putc((char)2); //assume packet type constant - debug.putc((char)0); //count seems unused - for(int i = 0; i < 16; i++){ //16 bytes for 4 32bit floats - debug.putc((char)(buffer[i])); - } - for(int i = 0; i < 5; i++){ //no idea, padded with 0 - debug.putc((char)0); - } - */ } } } + +/*///////////////////////////////////////////////////////////////////////////////////////////////////////// +// TIMER0 / CAPTURE0[0] +///////////////////////////////////////////////////////////////////////////////////////////////////////// + + int prevState_=0; + int currState_=0; +void TIMER0_IRQHandler __irq (void){ + int h1 = (LPC_GPIO1->FIOPIN>>26) & 0x1; + int h2 = (LPC_GPIO1->FIOPIN>>27) & 0x1; + //2-bit state. + currState_ = (h1 << 2) | (h2 << 1); + + if ((prevState_ == 0x0) && (currState_ == 0x2)) { + pulses_++; + prevState_ = currState_; + return; + } else if (prevState_ == 0x0 && currState_ == 0x1) { + pulses_--; + prevState_ = currState_; + return; + } + + if ((prevState_ == 0x2) && (currState_ == 0x3)) { + pulses_++; + prevState_ = currState_; + return; + } else if (prevState_ == 0x2 && currState_ == 0x0) { + pulses_--; + prevState_ = currState_; + return; + } + if ((prevState_ == 0x1) && (currState_ == 0x0)) { + pulses_++; + prevState_ = currState_; + return; + } else if (prevState_ == 0x1 && currState_ == 0x3) { + pulses_--; + prevState_ = currState_; + return; + } + + if ((prevState_ == 0x3) && (currState_ == 0x1)) { + pulses_++; + prevState_ = currState_; + return; + } else if (prevState_ == 0x3 && currState_ == 0x2) { + pulses_--; + prevState_ = currState_; + return; + } + + prevState_ = currState_; + if( LPC_TIM0->IR & 0x10 ) { + LPC_TIM0->IR = 0x10; // Clear MATx interrupt include DMA request + } + if( LPC_TIM0->IR & 0x20 ) { + LPC_TIM0->IR = 0x20; // Clear MATx interrupt include DMA request + } + +} + +void InitTimer0(void) +{ + //hold timer0 in reset + LPC_TIM0->TCR=2; + + //timer0 power on + LPC_SC->PCONP |= (0x01<<1); + + //cap0.0 cap0.1 pin input + LPC_PINCON->PINSEL3 &= ~((0x3<<20)|(0x3<<22)); + LPC_PINCON->PINSEL3 |= (0x3<<20)|(0x3<<22); + + //capture interrupts clear + LPC_TIM0->IR = 0x30; // Clear MATx interrupt include DMA request + + + //timer0 pid de kullanildigindan prescaler iptal. counter frekansi 18m olmali. + //LPC_TIM0->PR = pclk/1000000; // set prescaler to get 1 M counts/sec + + //Capture 0 and 1 on rising edge, interrupt enable. + LPC_TIM0->CCR = 0x3f;//(0x1<<0) | (0x1<<1) | (0x1<<2) | (0x1<<3) | (0x1<<4) |( 0x1<<5); + + NVIC_EnableIRQ(TIMER0_IRQn); + + //start timer0 + LPC_TIM0->TCR=1; +}*/ \ No newline at end of file