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@10:09dbd00164b9, 2015-05-13 (annotated)

Committer:
heuristics
Date:
Wed May 13 11:55:50 2015 +0000
Revision:
10:09dbd00164b9
Parent:
9:ca52462f9ebc
Child:
11:ba1a630e56c4
ad

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 10:09dbd00164b9 7 void InitTimer0(void);
heuristics 0:94acd21860e4 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 10:09dbd00164b9 14
heuristics 10:09dbd00164b9 15 volatile int pulses_ = 0;
heuristics 10:09dbd00164b9 16 InterruptIn hall1_(P0_5), hall2_(P0_4);
heuristics 10:09dbd00164b9 17 //Hallsensor hall1(P1_26, P1_27);
heuristics 10:09dbd00164b9 18 //Hallsensor hall2(P0_4, P0_5);
heuristics 10:09dbd00164b9 19
heuristics 10:09dbd00164b9 20 Motor motor1(P2_0, P2_1);
heuristics 10:09dbd00164b9 21 //Motor motor2(P2_2, P2_3, &pulses_);
heuristics 0:94acd21860e4 22
heuristics 4:7ccb10039316 23
heuristics 4:7ccb10039316 24 MPU9150 imu(P0_28, P0_27, P2_13);
heuristics 4:7ccb10039316 25
heuristics 4:7ccb10039316 26
heuristics 4:7ccb10039316 27 Serial RN42(P0_10, P0_11);
heuristics 4:7ccb10039316 28 Serial debug(P0_2, P0_3);
heuristics 4:7ccb10039316 29
heuristics 1:7681221d0a5c 30 Ticker infoTicker;
heuristics 4:7ccb10039316 31
heuristics 5:3953111e9476 32 Timer timer;
heuristics 5:3953111e9476 33 Timer timer2;
heuristics 5:3953111e9476 34
heuristics 7:7f9abf427b06 35
heuristics 7:7f9abf427b06 36
heuristics 4:7ccb10039316 37 char buffer[200];
heuristics 1:7681221d0a5c 38 void infoTask(void)
heuristics 1:7681221d0a5c 39 {
heuristics 1:7681221d0a5c 40 led1=!led1;
heuristics 1:7681221d0a5c 41 }
heuristics 10:09dbd00164b9 42 void deneme(void)
heuristics 10:09dbd00164b9 43 {
heuristics 10:09dbd00164b9 44 led3=!led3;
heuristics 10:09dbd00164b9 45 }
heuristics 1:7681221d0a5c 46 int main()
heuristics 1:7681221d0a5c 47 {
heuristics 10:09dbd00164b9 48 //Workout what the current state is.
heuristics 10:09dbd00164b9 49 int h1 = hall1_.read();
heuristics 10:09dbd00164b9 50 int h2 = hall2_.read();
heuristics 10:09dbd00164b9 51 // int h3 = hall3_.read();
heuristics 10:09dbd00164b9 52
heuristics 10:09dbd00164b9 53 // Set the PullUp Resistor
heuristics 10:09dbd00164b9 54 hall1_.mode(PullUp);
heuristics 10:09dbd00164b9 55 hall2_.mode(PullUp);
heuristics 10:09dbd00164b9 56 //hall3_.mode(PullUp);
heuristics 10:09dbd00164b9 57
heuristics 10:09dbd00164b9 58 //set interrupts on only hall 1-3 rise and fall.
heuristics 10:09dbd00164b9 59 hall1_.rise(&deneme);
heuristics 10:09dbd00164b9 60 hall1_.fall(&deneme);
heuristics 10:09dbd00164b9 61 hall2_.rise(&deneme);
heuristics 10:09dbd00164b9 62 hall2_.fall(&deneme);
heuristics 10:09dbd00164b9 63 //hall3_.rise(this, &deneme);
heuristics 10:09dbd00164b9 64 // hall3_.fall(this, &deneme);
heuristics 4:7ccb10039316 65 RN42.baud(9600);
heuristics 4:7ccb10039316 66 debug.baud(115200);
heuristics 10:09dbd00164b9 67 // InitTimer0();
heuristics 10:09dbd00164b9 68 // initialize_connection();
heuristics 6:3bd16fdf0b6e 69
heuristics 4:7ccb10039316 70 infoTicker.attach(infoTask,0.2f);
heuristics 4:7ccb10039316 71
heuristics 4:7ccb10039316 72 if(imu.isReady()) {
heuristics 4:7ccb10039316 73 debug.printf("MPU9150 is ready\r\n");
heuristics 4:7ccb10039316 74 } else {
heuristics 4:7ccb10039316 75 debug.printf("MPU9150 initialisation failure\r\n");
heuristics 4:7ccb10039316 76 }
heuristics 4:7ccb10039316 77
heuristics 4:7ccb10039316 78 imu.initialiseDMP();
heuristics 4:7ccb10039316 79
heuristics 4:7ccb10039316 80 timer.start();
heuristics 5:3953111e9476 81 timer2.start();
heuristics 4:7ccb10039316 82
heuristics 4:7ccb10039316 83 imu.setFifoReset(true);
heuristics 4:7ccb10039316 84 imu.setDMPEnabled(true);
heuristics 4:7ccb10039316 85
heuristics 4:7ccb10039316 86 Quaternion q1;
heuristics 0:94acd21860e4 87 while(1) {
heuristics 6:3bd16fdf0b6e 88
heuristics 4:7ccb10039316 89 if(imu.getFifoCount() >= 48) {
heuristics 4:7ccb10039316 90 imu.getFifoBuffer(buffer, 48);
heuristics 4:7ccb10039316 91 led2 = !led2;
heuristics 10:09dbd00164b9 92 //debug.printf("%d\r\n",timer2.read_ms());
heuristics 9:ca52462f9ebc 93 timer2.reset();
heuristics 4:7ccb10039316 94 }
heuristics 4:7ccb10039316 95
heuristics 7:7f9abf427b06 96 if(timer.read_ms() > 50) {
heuristics 4:7ccb10039316 97 timer.reset();
heuristics 4:7ccb10039316 98
heuristics 4:7ccb10039316 99 //This is the format of the data in the fifo,
heuristics 4:7ccb10039316 100 /* ================================================================================================ *
heuristics 4:7ccb10039316 101 | Default MotionApps v4.1 48-byte FIFO packet structure: |
heuristics 4:7ccb10039316 102 | |
heuristics 4:7ccb10039316 103 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
heuristics 4:7ccb10039316 104 | 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 105 | |
heuristics 4:7ccb10039316 106 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
heuristics 4:7ccb10039316 107 | 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 108 * ================================================================================================ */
heuristics 4:7ccb10039316 109
heuristics 4:7ccb10039316 110 /*
heuristics 4:7ccb10039316 111 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 4:7ccb10039316 112 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
heuristics 4:7ccb10039316 113 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
heuristics 4:7ccb10039316 114
heuristics 4:7ccb10039316 115 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 4:7ccb10039316 116 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
heuristics 4:7ccb10039316 117 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
heuristics 4:7ccb10039316 118
heuristics 4:7ccb10039316 119 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
heuristics 4:7ccb10039316 120 (int16_t)(buffer[31] << 8) + buffer[30],
heuristics 4:7ccb10039316 121 (int16_t)(buffer[33] << 8) + buffer[32]);
heuristics 4:7ccb10039316 122
heuristics 4:7ccb10039316 123 debug.printf("%f, %f, %f, %f\r\n",
heuristics 4:7ccb10039316 124 (float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + ((int32_t)buffer[2] << 8) + buffer[3]))* (1.0 / (1<<30)),
heuristics 4:7ccb10039316 125 (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + ((int32_t)buffer[6] << 8) + buffer[7]))* (1.0 / (1<<30)),
heuristics 4:7ccb10039316 126 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
heuristics 4:7ccb10039316 127 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
heuristics 4:7ccb10039316 128 */
heuristics 4:7ccb10039316 129
heuristics 4:7ccb10039316 130 q1.decode(buffer);
heuristics 10:09dbd00164b9 131
heuristics 10:09dbd00164b9 132 debug.printf("%f, ",hallSensor.read());
heuristics 7:7f9abf427b06 133 debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
heuristics 10:09dbd00164b9 134
heuristics 7:7f9abf427b06 135 Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
heuristics 8:63eff653d0ba 136 Vector3 pry=q1.getEulerAngles();
heuristics 10:09dbd00164b9 137 debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
heuristics 10:09dbd00164b9 138 /* debug.printf("m1: %d m2: %d \r\n ",motor1.getPulses(),motor2.getPulses());
heuristics 10:09dbd00164b9 139 motor1.setVelocity(pry.x/90);
heuristics 10:09dbd00164b9 140 motor2.setVelocity(pry.x/90);*/
heuristics 4:7ccb10039316 141
heuristics 4:7ccb10039316 142 }
heuristics 0:94acd21860e4 143 }
heuristics 0:94acd21860e4 144 }
heuristics 10:09dbd00164b9 145
heuristics 10:09dbd00164b9 146 /*/////////////////////////////////////////////////////////////////////////////////////////////////////////
heuristics 10:09dbd00164b9 147 // TIMER0 / CAPTURE0[0]
heuristics 10:09dbd00164b9 148 /////////////////////////////////////////////////////////////////////////////////////////////////////////
heuristics 10:09dbd00164b9 149
heuristics 10:09dbd00164b9 150 int prevState_=0;
heuristics 10:09dbd00164b9 151 int currState_=0;
heuristics 10:09dbd00164b9 152 void TIMER0_IRQHandler __irq (void){
heuristics 10:09dbd00164b9 153 int h1 = (LPC_GPIO1->FIOPIN>>26) & 0x1;
heuristics 10:09dbd00164b9 154 int h2 = (LPC_GPIO1->FIOPIN>>27) & 0x1;
heuristics 10:09dbd00164b9 155 //2-bit state.
heuristics 10:09dbd00164b9 156 currState_ = (h1 << 2) | (h2 << 1);
heuristics 10:09dbd00164b9 157
heuristics 10:09dbd00164b9 158 if ((prevState_ == 0x0) && (currState_ == 0x2)) {
heuristics 10:09dbd00164b9 159 pulses_++;
heuristics 10:09dbd00164b9 160 prevState_ = currState_;
heuristics 10:09dbd00164b9 161 return;
heuristics 10:09dbd00164b9 162 } else if (prevState_ == 0x0 && currState_ == 0x1) {
heuristics 10:09dbd00164b9 163 pulses_--;
heuristics 10:09dbd00164b9 164 prevState_ = currState_;
heuristics 10:09dbd00164b9 165 return;
heuristics 10:09dbd00164b9 166 }
heuristics 10:09dbd00164b9 167
heuristics 10:09dbd00164b9 168 if ((prevState_ == 0x2) && (currState_ == 0x3)) {
heuristics 10:09dbd00164b9 169 pulses_++;
heuristics 10:09dbd00164b9 170 prevState_ = currState_;
heuristics 10:09dbd00164b9 171 return;
heuristics 10:09dbd00164b9 172 } else if (prevState_ == 0x2 && currState_ == 0x0) {
heuristics 10:09dbd00164b9 173 pulses_--;
heuristics 10:09dbd00164b9 174 prevState_ = currState_;
heuristics 10:09dbd00164b9 175 return;
heuristics 10:09dbd00164b9 176 }
heuristics 10:09dbd00164b9 177 if ((prevState_ == 0x1) && (currState_ == 0x0)) {
heuristics 10:09dbd00164b9 178 pulses_++;
heuristics 10:09dbd00164b9 179 prevState_ = currState_;
heuristics 10:09dbd00164b9 180 return;
heuristics 10:09dbd00164b9 181 } else if (prevState_ == 0x1 && currState_ == 0x3) {
heuristics 10:09dbd00164b9 182 pulses_--;
heuristics 10:09dbd00164b9 183 prevState_ = currState_;
heuristics 10:09dbd00164b9 184 return;
heuristics 10:09dbd00164b9 185 }
heuristics 10:09dbd00164b9 186
heuristics 10:09dbd00164b9 187 if ((prevState_ == 0x3) && (currState_ == 0x1)) {
heuristics 10:09dbd00164b9 188 pulses_++;
heuristics 10:09dbd00164b9 189 prevState_ = currState_;
heuristics 10:09dbd00164b9 190 return;
heuristics 10:09dbd00164b9 191 } else if (prevState_ == 0x3 && currState_ == 0x2) {
heuristics 10:09dbd00164b9 192 pulses_--;
heuristics 10:09dbd00164b9 193 prevState_ = currState_;
heuristics 10:09dbd00164b9 194 return;
heuristics 10:09dbd00164b9 195 }
heuristics 10:09dbd00164b9 196
heuristics 10:09dbd00164b9 197 prevState_ = currState_;
heuristics 10:09dbd00164b9 198 if( LPC_TIM0->IR & 0x10 ) {
heuristics 10:09dbd00164b9 199 LPC_TIM0->IR = 0x10; // Clear MATx interrupt include DMA request
heuristics 10:09dbd00164b9 200 }
heuristics 10:09dbd00164b9 201 if( LPC_TIM0->IR & 0x20 ) {
heuristics 10:09dbd00164b9 202 LPC_TIM0->IR = 0x20; // Clear MATx interrupt include DMA request
heuristics 10:09dbd00164b9 203 }
heuristics 10:09dbd00164b9 204
heuristics 10:09dbd00164b9 205 }
heuristics 10:09dbd00164b9 206
heuristics 10:09dbd00164b9 207 void InitTimer0(void)
heuristics 10:09dbd00164b9 208 {
heuristics 10:09dbd00164b9 209 //hold timer0 in reset
heuristics 10:09dbd00164b9 210 LPC_TIM0->TCR=2;
heuristics 10:09dbd00164b9 211
heuristics 10:09dbd00164b9 212 //timer0 power on
heuristics 10:09dbd00164b9 213 LPC_SC->PCONP |= (0x01<<1);
heuristics 10:09dbd00164b9 214
heuristics 10:09dbd00164b9 215 //cap0.0 cap0.1 pin input
heuristics 10:09dbd00164b9 216 LPC_PINCON->PINSEL3 &= ~((0x3<<20)|(0x3<<22));
heuristics 10:09dbd00164b9 217 LPC_PINCON->PINSEL3 |= (0x3<<20)|(0x3<<22);
heuristics 10:09dbd00164b9 218
heuristics 10:09dbd00164b9 219 //capture interrupts clear
heuristics 10:09dbd00164b9 220 LPC_TIM0->IR = 0x30; // Clear MATx interrupt include DMA request
heuristics 10:09dbd00164b9 221
heuristics 10:09dbd00164b9 222
heuristics 10:09dbd00164b9 223 //timer0 pid de kullanildigindan prescaler iptal. counter frekansi 18m olmali.
heuristics 10:09dbd00164b9 224 //LPC_TIM0->PR = pclk/1000000; // set prescaler to get 1 M counts/sec
heuristics 10:09dbd00164b9 225
heuristics 10:09dbd00164b9 226 //Capture 0 and 1 on rising edge, interrupt enable.
heuristics 10:09dbd00164b9 227 LPC_TIM0->CCR = 0x3f;//(0x1<<0) | (0x1<<1) | (0x1<<2) | (0x1<<3) | (0x1<<4) |( 0x1<<5);
heuristics 10:09dbd00164b9 228
heuristics 10:09dbd00164b9 229 NVIC_EnableIRQ(TIMER0_IRQn);
heuristics 10:09dbd00164b9 230
heuristics 10:09dbd00164b9 231 //start timer0
heuristics 10:09dbd00164b9 232 LPC_TIM0->TCR=1;
heuristics 10:09dbd00164b9 233 }*/