Cool-step / Mbed 2 deprecated cool_step_1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MPU9150_DMP MotorControl QuaternionMath SimpleIOMacros mbed

Fork of cool_step_new by Cool-step

main.cpp@14:94a02b617085, 2015-05-14 (annotated)

Committer:
heuristics
Date:
Thu May 14 15:13:13 2015 +0000
Revision:
14:94a02b617085
Parent:
13:75892580d4ff
Child:
15:55b8bfa4d09c
just motor controlled

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 0:94acd21860e4 7
heuristics 4:7ccb10039316 8 //led pins
heuristics 1:7681221d0a5c 9 DigitalOut led1(P2_6);
heuristics 1:7681221d0a5c 10 DigitalOut led2(P2_7);
heuristics 1:7681221d0a5c 11 DigitalOut led3(P2_8);
heuristics 10:09dbd00164b9 12 AnalogIn hallSensor(P0_26);
heuristics 14:94a02b617085 13 //InterruptIn hall1_(P0_5), hall2_(P0_4);
heuristics 14:94a02b617085 14 //Hallsensor hall1(P1_26, P1_27);
heuristics 10:09dbd00164b9 15
heuristics 14:94a02b617085 16 //Motor motor1(P2_0, P2_1,&hall1);
heuristics 0:94acd21860e4 17
heuristics 4:7ccb10039316 18
heuristics 12:410ebe8573ce 19 //Serial RN42(P0_10, P0_11);
heuristics 4:7ccb10039316 20 Serial debug(P0_2, P0_3);
heuristics 4:7ccb10039316 21
heuristics 1:7681221d0a5c 22 Ticker infoTicker;
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 12:410ebe8573ce 28 Hallsensor hall2(P0_4, P0_5);
heuristics 12:410ebe8573ce 29 Motor motor2(P2_2, P2_3, &hall2);
heuristics 12:410ebe8573ce 30
heuristics 13:75892580d4ff 31 //MPU9150 imu(P0_28, P0_27, P2_13);
heuristics 7:7f9abf427b06 32
heuristics 4:7ccb10039316 33 char buffer[200];
heuristics 1:7681221d0a5c 34 void infoTask(void)
heuristics 1:7681221d0a5c 35 {
heuristics 1:7681221d0a5c 36 led1=!led1;
heuristics 1:7681221d0a5c 37 }
heuristics 10:09dbd00164b9 38 void deneme(void)
heuristics 10:09dbd00164b9 39 {
heuristics 10:09dbd00164b9 40 led3=!led3;
heuristics 10:09dbd00164b9 41 }
heuristics 1:7681221d0a5c 42 int main()
heuristics 1:7681221d0a5c 43 {
heuristics 11:ba1a630e56c4 44
heuristics 12:410ebe8573ce 45 // RN42.baud(9600);
heuristics 4:7ccb10039316 46 debug.baud(115200);
heuristics 10:09dbd00164b9 47 // InitTimer0();
heuristics 10:09dbd00164b9 48 // initialize_connection();
heuristics 6:3bd16fdf0b6e 49
heuristics 12:410ebe8573ce 50
heuristics 4:7ccb10039316 51 infoTicker.attach(infoTask,0.2f);
heuristics 4:7ccb10039316 52
heuristics 13:75892580d4ff 53 /* if(imu.isReady()) {
heuristics 4:7ccb10039316 54 debug.printf("MPU9150 is ready\r\n");
heuristics 4:7ccb10039316 55 } else {
heuristics 4:7ccb10039316 56 debug.printf("MPU9150 initialisation failure\r\n");
heuristics 4:7ccb10039316 57 }
heuristics 13:75892580d4ff 58 */
heuristics 13:75892580d4ff 59 // imu.initialiseDMP();
heuristics 4:7ccb10039316 60
heuristics 4:7ccb10039316 61 timer.start();
heuristics 5:3953111e9476 62 timer2.start();
heuristics 4:7ccb10039316 63
heuristics 14:94a02b617085 64 motor2.setVelocity(0.7);
heuristics 13:75892580d4ff 65 // imu.setFifoReset(true);
heuristics 13:75892580d4ff 66 // imu.setDMPEnabled(true);
heuristics 4:7ccb10039316 67
heuristics 13:75892580d4ff 68 // Quaternion q1;
heuristics 0:94acd21860e4 69 while(1) {
heuristics 6:3bd16fdf0b6e 70
heuristics 13:75892580d4ff 71 // if(imu.getFifoCount() >= 48) {
heuristics 13:75892580d4ff 72 // imu.getFifoBuffer(buffer, 48);
heuristics 13:75892580d4ff 73 // led2 = !led2;
heuristics 10:09dbd00164b9 74 //debug.printf("%d\r\n",timer2.read_ms());
heuristics 14:94a02b617085 75 // timer2.reset();
heuristics 14:94a02b617085 76 // }
heuristics 4:7ccb10039316 77
heuristics 14:94a02b617085 78 // if(timer.read_ms() > 50) {
heuristics 14:94a02b617085 79 // timer.reset();
heuristics 4:7ccb10039316 80
heuristics 4:7ccb10039316 81 //This is the format of the data in the fifo,
heuristics 4:7ccb10039316 82 /* ================================================================================================ *
heuristics 4:7ccb10039316 83 | Default MotionApps v4.1 48-byte FIFO packet structure: |
heuristics 4:7ccb10039316 84 | |
heuristics 4:7ccb10039316 85 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
heuristics 4:7ccb10039316 86 | 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 87 | |
heuristics 4:7ccb10039316 88 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
heuristics 4:7ccb10039316 89 | 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 90 * ================================================================================================ */
heuristics 4:7ccb10039316 91
heuristics 4:7ccb10039316 92 /*
heuristics 4:7ccb10039316 93 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 94 (int32_t)(((int32_t)buffer[38] << 24) + ((int32_t)buffer[39] << 16) + ((int32_t)buffer[40] << 8) + (int32_t)buffer[41]),
heuristics 4:7ccb10039316 95 (int32_t)(((int32_t)buffer[42] << 24) + ((int32_t)buffer[43] << 16) + ((int32_t)buffer[44] << 8) + (int32_t)buffer[45]));
heuristics 4:7ccb10039316 96
heuristics 4:7ccb10039316 97 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 98 (int32_t)(((int32_t)buffer[20] << 24) + ((int32_t)buffer[21] << 16) + ((int32_t)buffer[22] << 8) + (int32_t)buffer[23]),
heuristics 4:7ccb10039316 99 (int32_t)(((int32_t)buffer[24] << 24) + ((int32_t)buffer[25] << 16) + ((int32_t)buffer[26] << 8) + (int32_t)buffer[27]));
heuristics 4:7ccb10039316 100
heuristics 4:7ccb10039316 101 debug.printf("%d, %d, %d\r\n", (int16_t)(buffer[29] << 8) + buffer[28],
heuristics 4:7ccb10039316 102 (int16_t)(buffer[31] << 8) + buffer[30],
heuristics 4:7ccb10039316 103 (int16_t)(buffer[33] << 8) + buffer[32]);
heuristics 4:7ccb10039316 104
heuristics 4:7ccb10039316 105 debug.printf("%f, %f, %f, %f\r\n",
heuristics 4:7ccb10039316 106 (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 107 (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 108 (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 109 (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 110 */
heuristics 4:7ccb10039316 111
heuristics 13:75892580d4ff 112 // q1.decode(buffer);
heuristics 10:09dbd00164b9 113
heuristics 13:75892580d4ff 114 // debug.printf("%f, ",hallSensor.read());
heuristics 13:75892580d4ff 115 // debug.printf("%f, %f, %f, %f ", q1.w, q1.v.x, q1.v.y, q1.v.z);
heuristics 10:09dbd00164b9 116
heuristics 13:75892580d4ff 117 // Vector3 vector_x(1,0,0),vector_y(0,1,0),vector_z(0,0,1);
heuristics 13:75892580d4ff 118 // Vector3 pry=q1.getEulerAngles();
heuristics 13:75892580d4ff 119 // debug.printf("p: %f r: %f y: %f ",pry.x/PI*180,pry.y/PI*180,pry.z/PI*180);
heuristics 13:75892580d4ff 120 // debug.printf("m1: %d m2: %d \r\n ",motor2.getPulses(),motor2.getPulses());
heuristics 11:ba1a630e56c4 121 //motor1.setVelocity(pry.x/90);
heuristics 4:7ccb10039316 122
heuristics 4:7ccb10039316 123 }
heuristics 13:75892580d4ff 124
heuristics 14:94a02b617085 125 }