Cool-step
first publish
Dependencies: MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed
Fork of
cool_step
by 
Siner Gokhan Yilmaz
Diff: main.cpp
- Revision:
- 18:9f249b8a59ae
- Parent:
- 17:b3acd6416356
- Child:
- 19:e89f44519f6b
--- a/main.cpp Sat May 16 08:50:02 2015 +0000
+++ b/main.cpp Sat May 16 11:12:40 2015 +0000
@@ -4,11 +4,14 @@
#include "Motor.h"
+void initCaptures(void);
+
//led pins
DigitalOut led1(P2_6);
DigitalOut led2(P2_7);
DigitalOut led3(P2_8);
AnalogIn hallSensor(P0_26);
+int pulsesMotor2=0,pulsesMotor1=0;
//InterruptIn hall1_(P0_5), hall2_(P0_4);
@@ -21,15 +24,14 @@
Timer timer2;
-
MPU9150 imu(P0_28, P0_27, P2_13);
-Hallsensor hall2(P0_4, P0_5);
-Motor motor2(P2_2, P2_3, &hall2);
+//Hallsensor hall2(P0_4, P0_5);
+Motor motor2(P2_2, P2_3, NULL);
//Hallsensor hall1(P1_26, P1_27);
-//Motor motor1(P2_0, P2_1,&hall1);
+Motor motor1(P2_0, P2_1, NULL);
char buffer[200];
void infoTask(void)
{
@@ -41,20 +43,21 @@
}
int main()
{
-
+
RN42.baud(9600);
debug.baud(115200);
//InitTimer0();
- //initialize_connection();
+ //initialize_connection();
+
+ debug.printf("zaaa\r\n");
+ initCaptures();
-
+ debug.printf("zaaaxx\r\n");
infoTicker.attach(infoTask,0.2f);
- if(imu.isReady())
- {
+ if(imu.isReady()) {
debug.printf("MPU9150 is ready\r\n");
- } else
- {
+ } else {
debug.printf("MPU9150 initialisation failure\r\n");
}
@@ -67,17 +70,14 @@
imu.setDMPEnabled(true);
Quaternion q1;
-
- while(1)
- {
- if(imu.getFifoCount() >= 48)
- {
+
+ while(1) {
+ if(imu.getFifoCount() >= 48) {
imu.getFifoBuffer(buffer, 48);
led2 = !led2;
}
- if(timer.read_ms() > 50)
- {
+ if(timer.read_ms() > 50) {
timer.reset();
//This is the format of the data in the fifo,
@@ -91,25 +91,25 @@
| 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
* ================================================================================================ */
-
- /* debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]),
- (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
- (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
- debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]),
- (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
- (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
+ /* debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[34] << 24) + ((int32_t)buffer[35] << 16) + ((int32_t)buffer[36] << 8) + (int32_t)buffer[37]),
+ (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
+ (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
+
+ debug.printf("%d, %d, %d\r\n", (int32_t)(((int32_t)buffer[16] << 24) + ((int32_t)buffer[17] << 16) + ((int32_t)buffer[18] << 8) + (int32_t)buffer[19]),
+ (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
+ (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
- debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
- (int16_t)(buffer[31] << 8) + buffer[30],
- (int16_t)(buffer[33] << 8) + buffer[32]);
+ debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
+ (int16_t)(buffer[31] << 8) + buffer[30],
+ (int16_t)(buffer[33] << 8) + buffer[32]);
- debug.printf("%f, %f, %f, %f\r\n",
- (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
- (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
- (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
- (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
-*/
+ debug.printf("%f, %f, %f, %f\r\n",
+ (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
+ (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
+ (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
+ (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
+ */
q1.decode(buffer);
debug.printf("%f, ",hallSensor.read());
@@ -118,11 +118,66 @@
Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
Vector3 pry=q1.getEulerAngles();
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 ",motor2.getRevolutions(),motor2.getPulses());
- // motor1.setVelocity(pry.x/PI*2);
+ debug.printf("m1: %d m2: %d \r\n ",pulsesMotor1,pulsesMotor2);
+ motor1.setVelocity(pry.x/PI*2);
motor2.setVelocity(pry.x/PI*2);
}
}
-
+
+}
+
+
+
+
+void TIMER2_IRQHandler(void)
+{
+ int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1;
+ if(bitB) {
+ pulsesMotor2++;
+ } else {
+ pulsesMotor2--;
+ }
+
+ LPC_TIM2->IR |= 0x10;
+}
+
+void TIMER0_IRQHandler(void)
+{
+ int bitB = (LPC_GPIO1->FIOPIN>>27) & 0x1;
+ if(bitB) {
+ pulsesMotor1++;
+ } else {
+ pulsesMotor1--;
+ }
+
+ LPC_TIM2->IR |= 0x10;
}
+
+void initCaptures(void)
+{
+ LPC_PINCON->PINSEL0 |= 0x00000300; // set P0.4 to CAP2.0
+ LPC_SC->PCONP |= (1 << 22); // Timer2 power on
+ // init Timer 2 (cap2.0)
+ LPC_TIM2->IR = 0x10; // clear interrupt register
+ NVIC_SetVector(TIMER2_IRQn, uint32_t(TIMER2_IRQHandler));
+ LPC_TIM2->TC = 0; // clear timer counter
+ LPC_TIM2->PC = 0; // clear prescale counter
+ LPC_TIM2->PR = 0; // clear prescale register
+ LPC_TIM2->CCR |= 0x5; // enable cap2.0 rising capture; interrupt on cap2.0
+ LPC_TIM2->TCR = (1 << 0); // start Timer2
+ NVIC_EnableIRQ(TIMER2_IRQn);
+
+ LPC_PINCON->PINSEL3 |= 0x00030000; // set P1.26 to CAP0.0
+ LPC_SC->PCONP |= (1 << 1); // Timer0 power on
+ // init Timer 2 (cap0.0)
+ LPC_TIM0->IR = 0x10; // clear interrupt register
+ NVIC_SetVector(TIMER0_IRQn, uint32_t(TIMER0_IRQHandler));
+ LPC_TIM0->TC = 0; // clear timer counter
+ LPC_TIM0->PC = 0; // clear prescale counter
+ LPC_TIM0->PR = 0; // clear prescale register
+ LPC_TIM0->CCR |= 0x5; // enable cap0.0 rising capture; interrupt on cap0.0
+ LPC_TIM0->TCR = (1 << 0); // start Timer0
+ NVIC_EnableIRQ(TIMER0_IRQn);
+
+}
\ No newline at end of file