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@22:5cad60d669e6, 2015-05-26 (annotated)

Committer:
heuristics
Date:
Tue May 26 10:22:47 2015 +0000
Revision:
22:5cad60d669e6
Parent:
21:c5f88a11eac5
Child:
23:b79faf58d4a1
asdasd

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 22:5cad60d669e6 15 int int_time=0,bint_time=0;
heuristics 17:b3acd6416356 16 //InterruptIn hall1_(P0_5), hall2_(P0_4);
heuristics 0:94acd21860e4 17
heuristics 4:7ccb10039316 18
heuristics 15:55b8bfa4d09c 19 Serial RN42(P0_10, P0_11);
heuristics 4:7ccb10039316 20 Serial debug(P0_2, P0_3);
heuristics 4:7ccb10039316 21
heuristics 20:c0d085d64905 22 Ticker infoTicker,commandTicker;
heuristics 4:7ccb10039316 23
heuristics 5:3953111e9476 24 Timer timer;
heuristics 5:3953111e9476 25 Timer timer2;
heuristics 5:3953111e9476 26
heuristics 7:7f9abf427b06 27
heuristics 17:b3acd6416356 28 MPU9150 imu(P0_28, P0_27, P2_13);
heuristics 17:b3acd6416356 29
heuristics 18:9f249b8a59ae 30 //Hallsensor hall2(P0_4, P0_5);
heuristics 20:c0d085d64905 31 Motor motor2(P2_2, P2_3, pulsesMotor2);
heuristics 12:410ebe8573ce 32
heuristics 17:b3acd6416356 33 //Hallsensor hall1(P1_26, P1_27);
heuristics 7:7f9abf427b06 34
heuristics 20:c0d085d64905 35 Motor motor1(P2_0, P2_1, pulsesMotor1);
heuristics 4:7ccb10039316 36 char buffer[200];
heuristics 1:7681221d0a5c 37 void infoTask(void)
heuristics 1:7681221d0a5c 38 {
heuristics 1:7681221d0a5c 39 led1=!led1;
heuristics 1:7681221d0a5c 40 }
heuristics 20:c0d085d64905 41 int str2int(char* str)
heuristics 20:c0d085d64905 42 {
heuristics 20:c0d085d64905 43 int num = 0, i = 0;
heuristics 20:c0d085d64905 44 while(str[i] != '\n') {
heuristics 20:c0d085d64905 45 num = num * 10 + str[i] - '0';
heuristics 20:c0d085d64905 46 i++;
heuristics 20:c0d085d64905 47 }
heuristics 20:c0d085d64905 48 return num;
heuristics 20:c0d085d64905 49 }
heuristics 20:c0d085d64905 50 void getCommand(void)
heuristics 10:09dbd00164b9 51 {
heuristics 20:c0d085d64905 52 if(debug.readable()) {
heuristics 20:c0d085d64905 53 char buffer[128];
heuristics 20:c0d085d64905 54 debug.gets(buffer, 20);
heuristics 20:c0d085d64905 55 if(buffer[0]=='m') {
heuristics 20:c0d085d64905 56 float speed = (float)(str2int(buffer+3));
heuristics 20:c0d085d64905 57 debug.printf("FLOAT %f FLOAT ",speed);
heuristics 20:c0d085d64905 58 if(buffer[1]=='1') {
heuristics 20:c0d085d64905 59 motor1.setAngle(speed);
heuristics 20:c0d085d64905 60 }
heuristics 20:c0d085d64905 61
heuristics 20:c0d085d64905 62 if(buffer[1]=='2') {
heuristics 20:c0d085d64905 63
heuristics 20:c0d085d64905 64 motor2.setAngle(speed);
heuristics 20:c0d085d64905 65 }
heuristics 20:c0d085d64905 66
heuristics 20:c0d085d64905 67
heuristics 20:c0d085d64905 68 }
heuristics 20:c0d085d64905 69 }
heuristics 10:09dbd00164b9 70 }
heuristics 1:7681221d0a5c 71 int main()
heuristics 1:7681221d0a5c 72 {
heuristics 18:9f249b8a59ae 73
heuristics 15:55b8bfa4d09c 74 RN42.baud(9600);
heuristics 4:7ccb10039316 75 debug.baud(115200);
heuristics 15:55b8bfa4d09c 76 //InitTimer0();
heuristics 18:9f249b8a59ae 77 //initialize_connection();
heuristics 4:7ccb10039316 78 infoTicker.attach(infoTask,0.2f);
heuristics 20:c0d085d64905 79 commandTicker.attach(getCommand,0.2f);
heuristics 4:7ccb10039316 80
heuristics 18:9f249b8a59ae 81 if(imu.isReady()) {
heuristics 4:7ccb10039316 82 debug.printf("MPU9150 is ready\r\n");
heuristics 18:9f249b8a59ae 83 } else {
heuristics 4:7ccb10039316 84 debug.printf("MPU9150 initialisation failure\r\n");
heuristics 4:7ccb10039316 85 }
heuristics 15:55b8bfa4d09c 86
heuristics 15:55b8bfa4d09c 87 imu.initialiseDMP();
heuristics 4:7ccb10039316 88
heuristics 4:7ccb10039316 89 timer.start();
heuristics 22:5cad60d669e6 90 timer2.start();
heuristics 4:7ccb10039316 91
heuristics 17:b3acd6416356 92 //motor2.setVelocity(0.7);
heuristics 15:55b8bfa4d09c 93 imu.setFifoReset(true);
heuristics 15:55b8bfa4d09c 94 imu.setDMPEnabled(true);
heuristics 4:7ccb10039316 95
heuristics 15:55b8bfa4d09c 96 Quaternion q1;
heuristics 22:5cad60d669e6 97 wait_ms(500);
heuristics 21:c5f88a11eac5 98 motor1.setAngle(0);
heuristics 21:c5f88a11eac5 99
heuristics 21:c5f88a11eac5 100 motor2.setAngle(0);
heuristics 20:c0d085d64905 101 initCaptures();
heuristics 22:5cad60d669e6 102 motor1.setAngle(0);
heuristics 22:5cad60d669e6 103 motor2.setAngle(0);
heuristics 20:c0d085d64905 104
heuristics 20:c0d085d64905 105
heuristics 18:9f249b8a59ae 106 while(1) {
heuristics 18:9f249b8a59ae 107 if(imu.getFifoCount() >= 48) {
heuristics 15:55b8bfa4d09c 108 imu.getFifoBuffer(buffer, 48);
heuristics 15:55b8bfa4d09c 109 led2 = !led2;
heuristics 15:55b8bfa4d09c 110 }
heuristics 4:7ccb10039316 111
heuristics 18:9f249b8a59ae 112 if(timer.read_ms() > 50) {
heuristics 15:55b8bfa4d09c 113 timer.reset();
heuristics 4:7ccb10039316 114
heuristics 4:7ccb10039316 115 //This is the format of the data in the fifo,
heuristics 4:7ccb10039316 116 /* ================================================================================================ *
heuristics 4:7ccb10039316 117 | Default MotionApps v4.1 48-byte FIFO packet structure: |
heuristics 4:7ccb10039316 118 | |
heuristics 4:7ccb10039316 119 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
heuristics 4:7ccb10039316 120 | 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 121 | |
heuristics 4:7ccb10039316 122 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
heuristics 4:7ccb10039316 123 | 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 124 * ================================================================================================ */
heuristics 4:7ccb10039316 125
heuristics 4:7ccb10039316 126
heuristics 18:9f249b8a59ae 127 /* 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 128 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
heuristics 18:9f249b8a59ae 129 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
heuristics 18:9f249b8a59ae 130
heuristics 18:9f249b8a59ae 131 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 132 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
heuristics 18:9f249b8a59ae 133 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
heuristics 4:7ccb10039316 134
heuristics 18:9f249b8a59ae 135 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
heuristics 18:9f249b8a59ae 136 (int16_t)(buffer[31] << 8) + buffer[30],
heuristics 18:9f249b8a59ae 137 (int16_t)(buffer[33] << 8) + buffer[32]);
heuristics 4:7ccb10039316 138
heuristics 18:9f249b8a59ae 139 debug.printf("%f, %f, %f, %f\r\n",
heuristics 18:9f249b8a59ae 140 (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 141 (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 142 (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 143 (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 144 */
heuristics 15:55b8bfa4d09c 145 q1.decode(buffer);
heuristics 10:09dbd00164b9 146
heuristics 15:55b8bfa4d09c 147 debug.printf("%f, ",hallSensor.read());
heuristics 15:55b8bfa4d09c 148 debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
heuristics 10:09dbd00164b9 149
heuristics 15:55b8bfa4d09c 150 Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
heuristics 15:55b8bfa4d09c 151 Vector3 pry=q1.getEulerAngles();
heuristics 15:55b8bfa4d09c 152 debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
heuristics 20:c0d085d64905 153 debug.printf("m1: %.2f m2: %.2f ",motor1.getAngle(),motor2.getAngle());
heuristics 22:5cad60d669e6 154 debug.printf("dt1: %f dt2: %f ",motor1.get_dt(),motor2.get_dt());
heuristics 22:5cad60d669e6 155 debug.printf("Direction: %d Int Time: %d \r\n ",bint_time,int_time);
heuristics 22:5cad60d669e6 156 /* if(!motor1.isRunning()) {
heuristics 20:c0d085d64905 157 if(motor1.getAngle()>30) {
heuristics 20:c0d085d64905 158 motor1.setAngle(0);
heuristics 20:c0d085d64905 159 }
heuristics 20:c0d085d64905 160 if(motor1.getAngle()<30) {
heuristics 20:c0d085d64905 161 motor1.setAngle(60);
heuristics 20:c0d085d64905 162 }
heuristics 20:c0d085d64905 163 }
heuristics 22:5cad60d669e6 164 if(!motor2.isRunning()) {
heuristics 20:c0d085d64905 165 if(motor2.getAngle()>30) {
heuristics 20:c0d085d64905 166 motor2.setAngle(0);
heuristics 20:c0d085d64905 167 }
heuristics 20:c0d085d64905 168 if(motor2.getAngle()<30) {
heuristics 20:c0d085d64905 169 motor2.setAngle(60);
heuristics 20:c0d085d64905 170 }
heuristics 20:c0d085d64905 171 }*/
heuristics 4:7ccb10039316 172 }
heuristics 15:55b8bfa4d09c 173 }
heuristics 18:9f249b8a59ae 174
heuristics 18:9f249b8a59ae 175 }
heuristics 18:9f249b8a59ae 176
heuristics 18:9f249b8a59ae 177
heuristics 18:9f249b8a59ae 178
heuristics 18:9f249b8a59ae 179
heuristics 19:e89f44519f6b 180
heuristics 19:e89f44519f6b 181
heuristics 19:e89f44519f6b 182
heuristics 19:e89f44519f6b 183
heuristics 19:e89f44519f6b 184
heuristics 19:e89f44519f6b 185
heuristics 19:e89f44519f6b 186
heuristics 19:e89f44519f6b 187
heuristics 19:e89f44519f6b 188
heuristics 19:e89f44519f6b 189
heuristics 19:e89f44519f6b 190
heuristics 19:e89f44519f6b 191
heuristics 19:e89f44519f6b 192
heuristics 19:e89f44519f6b 193
heuristics 19:e89f44519f6b 194
heuristics 19:e89f44519f6b 195
heuristics 19:e89f44519f6b 196
heuristics 19:e89f44519f6b 197
heuristics 19:e89f44519f6b 198
heuristics 19:e89f44519f6b 199
heuristics 19:e89f44519f6b 200
heuristics 19:e89f44519f6b 201
heuristics 19:e89f44519f6b 202
heuristics 19:e89f44519f6b 203
heuristics 19:e89f44519f6b 204
heuristics 19:e89f44519f6b 205
heuristics 19:e89f44519f6b 206
heuristics 19:e89f44519f6b 207
heuristics 19:e89f44519f6b 208
heuristics 19:e89f44519f6b 209
heuristics 19:e89f44519f6b 210
heuristics 19:e89f44519f6b 211
heuristics 19:e89f44519f6b 212
heuristics 19:e89f44519f6b 213
heuristics 19:e89f44519f6b 214
heuristics 19:e89f44519f6b 215
heuristics 19:e89f44519f6b 216
heuristics 19:e89f44519f6b 217
heuristics 18:9f249b8a59ae 218 void TIMER2_IRQHandler(void)
heuristics 18:9f249b8a59ae 219 {
heuristics 22:5cad60d669e6 220 // timer2.reset();
heuristics 18:9f249b8a59ae 221 int bitB = (LPC_GPIO0->FIOPIN>>5) & 0x1;
heuristics 18:9f249b8a59ae 222 if(bitB) {
heuristics 20:c0d085d64905 223 pulsesMotor2--;
heuristics 22:5cad60d669e6 224 int_time=0;
heuristics 18:9f249b8a59ae 225 } else {
heuristics 20:c0d085d64905 226 pulsesMotor2++;
heuristics 22:5cad60d669e6 227 int_time=1;
heuristics 18:9f249b8a59ae 228 }
heuristics 18:9f249b8a59ae 229
heuristics 18:9f249b8a59ae 230 LPC_TIM2->IR |= 0x10;
heuristics 22:5cad60d669e6 231 //bint_time=timer2.read_us();
heuristics 18:9f249b8a59ae 232 }
heuristics 18:9f249b8a59ae 233
heuristics 18:9f249b8a59ae 234 void TIMER0_IRQHandler(void)
heuristics 18:9f249b8a59ae 235 {
heuristics 18:9f249b8a59ae 236 int bitB = (LPC_GPIO1->FIOPIN>>27) & 0x1;
heuristics 18:9f249b8a59ae 237 if(bitB) {
heuristics 20:c0d085d64905 238 pulsesMotor1--;
heuristics 18:9f249b8a59ae 239 } else {
heuristics 20:c0d085d64905 240 pulsesMotor1++;
heuristics 18:9f249b8a59ae 241 }
heuristics 18:9f249b8a59ae 242
heuristics 19:e89f44519f6b 243 LPC_TIM0->IR |= 0x10;
heuristics 14:94a02b617085 244 }
heuristics 18:9f249b8a59ae 245
heuristics 18:9f249b8a59ae 246 void initCaptures(void)
heuristics 18:9f249b8a59ae 247 {
heuristics 22:5cad60d669e6 248 NVIC_SetPriority (TIMER3_IRQn, 6);
heuristics 18:9f249b8a59ae 249 LPC_PINCON->PINSEL0 |= 0x00000300; // set P0.4 to CAP2.0
heuristics 18:9f249b8a59ae 250 LPC_SC->PCONP |= (1 << 22); // Timer2 power on
heuristics 18:9f249b8a59ae 251 // init Timer 2 (cap2.0)
heuristics 18:9f249b8a59ae 252 LPC_TIM2->IR = 0x10; // clear interrupt register
heuristics 18:9f249b8a59ae 253 NVIC_SetVector(TIMER2_IRQn, uint32_t(TIMER2_IRQHandler));
heuristics 20:c0d085d64905 254 LPC_TIM2->TC = 0; // clear timer counter
heuristics 18:9f249b8a59ae 255 LPC_TIM2->PC = 0; // clear prescale counter
heuristics 18:9f249b8a59ae 256 LPC_TIM2->PR = 0; // clear prescale register
heuristics 18:9f249b8a59ae 257 LPC_TIM2->CCR |= 0x5; // enable cap2.0 rising capture; interrupt on cap2.0
heuristics 22:5cad60d669e6 258 NVIC_SetPriority (TIMER2_IRQn, 5);
heuristics 18:9f249b8a59ae 259 LPC_TIM2->TCR = (1 << 0); // start Timer2
heuristics 18:9f249b8a59ae 260 NVIC_EnableIRQ(TIMER2_IRQn);
heuristics 20:c0d085d64905 261
heuristics 19:e89f44519f6b 262 LPC_PINCON->PINSEL3 |= 0x00300000; // set P1.26 to CAP0.0
heuristics 18:9f249b8a59ae 263 LPC_SC->PCONP |= (1 << 1); // Timer0 power on
heuristics 22:5cad60d669e6 264 // init Timer 0 (cap0.0)
heuristics 18:9f249b8a59ae 265 LPC_TIM0->IR = 0x10; // clear interrupt register
heuristics 18:9f249b8a59ae 266 NVIC_SetVector(TIMER0_IRQn, uint32_t(TIMER0_IRQHandler));
heuristics 20:c0d085d64905 267 LPC_TIM0->TC = 0; // clear timer counter
heuristics 18:9f249b8a59ae 268 LPC_TIM0->PC = 0; // clear prescale counter
heuristics 18:9f249b8a59ae 269 LPC_TIM0->PR = 0; // clear prescale register
heuristics 18:9f249b8a59ae 270 LPC_TIM0->CCR |= 0x5; // enable cap0.0 rising capture; interrupt on cap0.0
heuristics 22:5cad60d669e6 271 NVIC_SetPriority (TIMER0_IRQn, 5);
heuristics 18:9f249b8a59ae 272 LPC_TIM0->TCR = (1 << 0); // start Timer0
heuristics 18:9f249b8a59ae 273 NVIC_EnableIRQ(TIMER0_IRQn);
heuristics 18:9f249b8a59ae 274
heuristics 18:9f249b8a59ae 275 }