Cool-step / Mbed 2 deprecated cool_step_new

first publish

Dependencies:   MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed

Fork of cool_step by Siner Gokhan Yilmaz

main.cpp@19:e89f44519f6b, 2015-05-16 (annotated)

Committer:
heuristics
Date:
Sat May 16 11:50:27 2015 +0000
Revision:
19:e89f44519f6b
Parent:
18:9f249b8a59ae
Child:
20:c0d085d64905
Motor1+2 calisiyor, encoderlar da canavar, zaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
heuristics 0:94acd21860e4 1 #include "mbed.h"
heuristics 4:7ccb10039316 2 #include "MPU9150.h"
heuristics 4:7ccb10039316 3 #include "Quaternion.h"
heuristics 10:09dbd00164b9 4 #include "Motor.h"
heuristics 10:09dbd00164b9 5
heuristics 10:09dbd00164b9 6
heuristics 18:9f249b8a59ae 7 void initCaptures(void);
heuristics 18:9f249b8a59ae 8
heuristics 4:7ccb10039316 9 //led pins
heuristics 1:7681221d0a5c 10 DigitalOut led1(P2_6);
heuristics 1:7681221d0a5c 11 DigitalOut led2(P2_7);
heuristics 1:7681221d0a5c 12 DigitalOut led3(P2_8);
heuristics 10:09dbd00164b9 13 AnalogIn hallSensor(P0_26);
heuristics 18:9f249b8a59ae 14 int pulsesMotor2=0,pulsesMotor1=0;
heuristics 17:b3acd6416356 15 //InterruptIn hall1_(P0_5), hall2_(P0_4);
heuristics 0:94acd21860e4 16
heuristics 4:7ccb10039316 17
heuristics 15:55b8bfa4d09c 18 Serial RN42(P0_10, P0_11);
heuristics 4:7ccb10039316 19 Serial debug(P0_2, P0_3);
heuristics 4:7ccb10039316 20
heuristics 1:7681221d0a5c 21 Ticker infoTicker;
heuristics 4:7ccb10039316 22
heuristics 5:3953111e9476 23 Timer timer;
heuristics 5:3953111e9476 24 Timer timer2;
heuristics 5:3953111e9476 25
heuristics 7:7f9abf427b06 26
heuristics 17:b3acd6416356 27 MPU9150 imu(P0_28, P0_27, P2_13);
heuristics 17:b3acd6416356 28
heuristics 18:9f249b8a59ae 29 //Hallsensor hall2(P0_4, P0_5);
heuristics 18:9f249b8a59ae 30 Motor motor2(P2_2, P2_3, NULL);
heuristics 12:410ebe8573ce 31
heuristics 17:b3acd6416356 32 //Hallsensor hall1(P1_26, P1_27);
heuristics 7:7f9abf427b06 33
heuristics 18:9f249b8a59ae 34 Motor motor1(P2_0, P2_1, NULL);
heuristics 4:7ccb10039316 35 char buffer[200];
heuristics 1:7681221d0a5c 36 void infoTask(void)
heuristics 1:7681221d0a5c 37 {
heuristics 1:7681221d0a5c 38 led1=!led1;
heuristics 1:7681221d0a5c 39 }
heuristics 10:09dbd00164b9 40 void deneme(void)
heuristics 10:09dbd00164b9 41 {
heuristics 10:09dbd00164b9 42 led3=!led3;
heuristics 10:09dbd00164b9 43 }
heuristics 1:7681221d0a5c 44 int main()
heuristics 1:7681221d0a5c 45 {
heuristics 18:9f249b8a59ae 46
heuristics 15:55b8bfa4d09c 47 RN42.baud(9600);
heuristics 4:7ccb10039316 48 debug.baud(115200);
heuristics 15:55b8bfa4d09c 49 //InitTimer0();
heuristics 18:9f249b8a59ae 50 //initialize_connection();
heuristics 18:9f249b8a59ae 51
heuristics 18:9f249b8a59ae 52 debug.printf("zaaa\r\n");
heuristics 18:9f249b8a59ae 53 initCaptures();
heuristics 6:3bd16fdf0b6e 54
heuristics 18:9f249b8a59ae 55 debug.printf("zaaaxx\r\n");
heuristics 4:7ccb10039316 56 infoTicker.attach(infoTask,0.2f);
heuristics 4:7ccb10039316 57
heuristics 18:9f249b8a59ae 58 if(imu.isReady()) {
heuristics 4:7ccb10039316 59 debug.printf("MPU9150 is ready\r\n");
heuristics 18:9f249b8a59ae 60 } else {
heuristics 4:7ccb10039316 61 debug.printf("MPU9150 initialisation failure\r\n");
heuristics 4:7ccb10039316 62 }
heuristics 15:55b8bfa4d09c 63
heuristics 15:55b8bfa4d09c 64 imu.initialiseDMP();
heuristics 4:7ccb10039316 65
heuristics 4:7ccb10039316 66 timer.start();
heuristics 4:7ccb10039316 67
heuristics 17:b3acd6416356 68 //motor2.setVelocity(0.7);
heuristics 15:55b8bfa4d09c 69 imu.setFifoReset(true);
heuristics 15:55b8bfa4d09c 70 imu.setDMPEnabled(true);
heuristics 4:7ccb10039316 71
heuristics 15:55b8bfa4d09c 72 Quaternion q1;
heuristics 18:9f249b8a59ae 73
heuristics 18:9f249b8a59ae 74 while(1) {
heuristics 18:9f249b8a59ae 75 if(imu.getFifoCount() >= 48) {
heuristics 15:55b8bfa4d09c 76 imu.getFifoBuffer(buffer, 48);
heuristics 15:55b8bfa4d09c 77 led2 = !led2;
heuristics 15:55b8bfa4d09c 78 }
heuristics 4:7ccb10039316 79
heuristics 18:9f249b8a59ae 80 if(timer.read_ms() > 50) {
heuristics 15:55b8bfa4d09c 81 timer.reset();
heuristics 4:7ccb10039316 82
heuristics 4:7ccb10039316 83 //This is the format of the data in the fifo,
heuristics 4:7ccb10039316 84 /* ================================================================================================ *
heuristics 4:7ccb10039316 85 | Default MotionApps v4.1 48-byte FIFO packet structure: |
heuristics 4:7ccb10039316 86 | |
heuristics 4:7ccb10039316 87 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
heuristics 4:7ccb10039316 88 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
heuristics 4:7ccb10039316 89 | |
heuristics 4:7ccb10039316 90 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
heuristics 4:7ccb10039316 91 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
heuristics 4:7ccb10039316 92 * ================================================================================================ */
heuristics 4:7ccb10039316 93
heuristics 4:7ccb10039316 94
heuristics 18:9f249b8a59ae 95 /* 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]),
heuristics 18:9f249b8a59ae 96 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
heuristics 18:9f249b8a59ae 97 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
heuristics 18:9f249b8a59ae 98
heuristics 18:9f249b8a59ae 99 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]),
heuristics 18:9f249b8a59ae 100 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
heuristics 18:9f249b8a59ae 101 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
heuristics 4:7ccb10039316 102
heuristics 18:9f249b8a59ae 103 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
heuristics 18:9f249b8a59ae 104 (int16_t)(buffer[31] << 8) + buffer[30],
heuristics 18:9f249b8a59ae 105 (int16_t)(buffer[33] << 8) + buffer[32]);
heuristics 4:7ccb10039316 106
heuristics 18:9f249b8a59ae 107 debug.printf("%f, %f, %f, %f\r\n",
heuristics 18:9f249b8a59ae 108 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
heuristics 18:9f249b8a59ae 109 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
heuristics 18:9f249b8a59ae 110 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
heuristics 18:9f249b8a59ae 111 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
heuristics 18:9f249b8a59ae 112 */
heuristics 15:55b8bfa4d09c 113 q1.decode(buffer);
heuristics 10:09dbd00164b9 114
heuristics 15:55b8bfa4d09c 115 debug.printf("%f, ",hallSensor.read());
heuristics 15:55b8bfa4d09c 116 debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
heuristics 10:09dbd00164b9 117
heuristics 15:55b8bfa4d09c 118 Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
heuristics 15:55b8bfa4d09c 119 Vector3 pry=q1.getEulerAngles();
heuristics 15:55b8bfa4d09c 120 debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
heuristics 18:9f249b8a59ae 121 debug.printf("m1: %d m2: %d \r\n ",pulsesMotor1,pulsesMotor2);
heuristics 18:9f249b8a59ae 122 motor1.setVelocity(pry.x/PI*2);
heuristics 17:b3acd6416356 123 motor2.setVelocity(pry.x/PI*2);
heuristics 4:7ccb10039316 124
heuristics 4:7ccb10039316 125 }
heuristics 15:55b8bfa4d09c 126 }
heuristics 18:9f249b8a59ae 127
heuristics 18:9f249b8a59ae 128 }
heuristics 18:9f249b8a59ae 129
heuristics 18:9f249b8a59ae 130
heuristics 18:9f249b8a59ae 131
heuristics 18:9f249b8a59ae 132
heuristics 19:e89f44519f6b 133
heuristics 19:e89f44519f6b 134
heuristics 19:e89f44519f6b 135
heuristics 19:e89f44519f6b 136
heuristics 19:e89f44519f6b 137
heuristics 19:e89f44519f6b 138
heuristics 19:e89f44519f6b 139
heuristics 19:e89f44519f6b 140
heuristics 19:e89f44519f6b 141
heuristics 19:e89f44519f6b 142
heuristics 19:e89f44519f6b 143
heuristics 19:e89f44519f6b 144
heuristics 19:e89f44519f6b 145
heuristics 19:e89f44519f6b 146
heuristics 19:e89f44519f6b 147
heuristics 19:e89f44519f6b 148
heuristics 19:e89f44519f6b 149
heuristics 19:e89f44519f6b 150
heuristics 19:e89f44519f6b 151
heuristics 19:e89f44519f6b 152
heuristics 19:e89f44519f6b 153
heuristics 19:e89f44519f6b 154
heuristics 19:e89f44519f6b 155
heuristics 19:e89f44519f6b 156
heuristics 19:e89f44519f6b 157
heuristics 19:e89f44519f6b 158
heuristics 19:e89f44519f6b 159
heuristics 19:e89f44519f6b 160
heuristics 19:e89f44519f6b 161
heuristics 19:e89f44519f6b 162
heuristics 19:e89f44519f6b 163
heuristics 19:e89f44519f6b 164
heuristics 19:e89f44519f6b 165
heuristics 19:e89f44519f6b 166
heuristics 19:e89f44519f6b 167
heuristics 19:e89f44519f6b 168
heuristics 19:e89f44519f6b 169
heuristics 19:e89f44519f6b 170
heuristics 18:9f249b8a59ae 171 void TIMER2_IRQHandler(void)
heuristics 18:9f249b8a59ae 172 {
heuristics 18:9f249b8a59ae 173 int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1;
heuristics 18:9f249b8a59ae 174 if(bitB) {
heuristics 18:9f249b8a59ae 175 pulsesMotor2++;
heuristics 18:9f249b8a59ae 176 } else {
heuristics 18:9f249b8a59ae 177 pulsesMotor2--;
heuristics 18:9f249b8a59ae 178 }
heuristics 18:9f249b8a59ae 179
heuristics 18:9f249b8a59ae 180 LPC_TIM2->IR |= 0x10;
heuristics 18:9f249b8a59ae 181 }
heuristics 18:9f249b8a59ae 182
heuristics 18:9f249b8a59ae 183 void TIMER0_IRQHandler(void)
heuristics 18:9f249b8a59ae 184 {
heuristics 18:9f249b8a59ae 185 int bitB = (LPC_GPIO1->FIOPIN>>27) & 0x1;
heuristics 18:9f249b8a59ae 186 if(bitB) {
heuristics 18:9f249b8a59ae 187 pulsesMotor1++;
heuristics 18:9f249b8a59ae 188 } else {
heuristics 18:9f249b8a59ae 189 pulsesMotor1--;
heuristics 18:9f249b8a59ae 190 }
heuristics 18:9f249b8a59ae 191
heuristics 19:e89f44519f6b 192 LPC_TIM0->IR |= 0x10;
heuristics 14:94a02b617085 193 }
heuristics 18:9f249b8a59ae 194
heuristics 18:9f249b8a59ae 195 void initCaptures(void)
heuristics 18:9f249b8a59ae 196 {
heuristics 18:9f249b8a59ae 197 LPC_PINCON->PINSEL0 |= 0x00000300; // set P0.4 to CAP2.0
heuristics 18:9f249b8a59ae 198 LPC_SC->PCONP |= (1 << 22); // Timer2 power on
heuristics 18:9f249b8a59ae 199 // init Timer 2 (cap2.0)
heuristics 18:9f249b8a59ae 200 LPC_TIM2->IR = 0x10; // clear interrupt register
heuristics 18:9f249b8a59ae 201 NVIC_SetVector(TIMER2_IRQn, uint32_t(TIMER2_IRQHandler));
heuristics 18:9f249b8a59ae 202 LPC_TIM2->TC = 0; // clear timer counter
heuristics 18:9f249b8a59ae 203 LPC_TIM2->PC = 0; // clear prescale counter
heuristics 18:9f249b8a59ae 204 LPC_TIM2->PR = 0; // clear prescale register
heuristics 18:9f249b8a59ae 205 LPC_TIM2->CCR |= 0x5; // enable cap2.0 rising capture; interrupt on cap2.0
heuristics 18:9f249b8a59ae 206 LPC_TIM2->TCR = (1 << 0); // start Timer2
heuristics 18:9f249b8a59ae 207 NVIC_EnableIRQ(TIMER2_IRQn);
heuristics 18:9f249b8a59ae 208
heuristics 19:e89f44519f6b 209 LPC_PINCON->PINSEL3 |= 0x00300000; // set P1.26 to CAP0.0
heuristics 18:9f249b8a59ae 210 LPC_SC->PCONP |= (1 << 1); // Timer0 power on
heuristics 18:9f249b8a59ae 211 // init Timer 2 (cap0.0)
heuristics 18:9f249b8a59ae 212 LPC_TIM0->IR = 0x10; // clear interrupt register
heuristics 18:9f249b8a59ae 213 NVIC_SetVector(TIMER0_IRQn, uint32_t(TIMER0_IRQHandler));
heuristics 18:9f249b8a59ae 214 LPC_TIM0->TC = 0; // clear timer counter
heuristics 18:9f249b8a59ae 215 LPC_TIM0->PC = 0; // clear prescale counter
heuristics 18:9f249b8a59ae 216 LPC_TIM0->PR = 0; // clear prescale register
heuristics 18:9f249b8a59ae 217 LPC_TIM0->CCR |= 0x5; // enable cap0.0 rising capture; interrupt on cap0.0
heuristics 18:9f249b8a59ae 218 LPC_TIM0->TCR = (1 << 0); // start Timer0
heuristics 18:9f249b8a59ae 219 NVIC_EnableIRQ(TIMER0_IRQn);
heuristics 18:9f249b8a59ae 220
heuristics 18:9f249b8a59ae 221 }