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@25:af166a728c89, 2015-06-12 (annotated)

Committer:
heuristics
Date:
Fri Jun 12 06:42:56 2015 +0000
Revision:
25:af166a728c89
Parent:
24:fa52fd530d6e 
12.6.2015

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