root / arm / main.c @ dfb90929
History | View | Annotate | Download (14.2 kB)
1 #include <stdio.h> 2 #include "drivers/mss_uart/mss_uart.h" 3 #include "drivers/CoreI2C/core_i2c.h" 4 #include "drivers/mss_gpio/mss_gpio.h" 5 #include "drivers/L3G4200D.h" 6 #include "drivers/LSM303.h" 7 #include "drivers/mss_timer/mss_timer.h" 8 //#include "PetPipe/PetPipe.h" 9 //#include "PetPipe/Integrator.h" 10 #include "drivers/A4988.h" 11 12 // constant values 13 #define DEGREESTOSTEPS 0.555555556 14 #define GYRO_OFFSET (0) 15 #define GYRO_READ_FREQUENCY ((uint32_t)0.005e8) 16 #define ROLLING_AWAY_MOTOR_DELAY (0x3FFF ) 17 #define ROLLING_BACK_MOTOR_DELAY (0x4FFF ) 18 19 // the number of steps to jump up when it is trying to speed up 20 #define SPEED_UP_STEPS (50) 21 22 // the amount to time to spend in the 'speedup' phase before 23 // entering a 'cooldown' phase to realign the motor and stuff 24 #define SPEEDUP_TIMEOUT ((uint32_t)10.0e8) 25 #define SPEEDUP_TIMEOUT_COOLDOWN ((uint32_t)0.080e8) 26 27 // the max speed the pipe will try to accelerate to 28 const int32_t RETURN_SPEED_UPPER = 3200; 29 // when the pipe reaches this speed, it will enter acceleration mode. 30 const int32_t RETURN_SPEED_LOWER = 800; 31 // while the pipe is rolling faster then this, it will not enter return mode 32 const int32_t LEAVING_SPEED_MIN = 200; 33 // 'home' will have an area where the pipe will feel 'home' as long as it 34 // is close enough 35 const int32_t HOME_THRESHOLD_CLOSE = 180; 36 const int32_t HOME_THRESHOLD_FAR = 200; 37 38 // when braking, when the speed gets this slow, it will be considered 'braked' 39 const int32_t BRAKING_MINIMUM = 50; 40 const int32_t BRAKING_UPPER_SLOWDOWN = 800; 41 42 43 44 45 enum states {WAIT, 46 ROLLING_AWAY, 47 TRACK_RETURN, 48 TRACK_SPEEDUP, 49 PUSH_BACK_TRANSITION, 50 BRAKING, 51 TRACK_SPEEDUP_TIMEOUT}; 52 53 54 // Global data structures 55 i2c_instance_t imu; 56 volatile gyro g; 57 volatile accelerometer a; 58 volatile magnetometer m; 59 60 volatile float integratedReturn = 720.0f; 61 volatile float integrated = 0.0f; 62 volatile float degreesSpunSinceLast = 0.0f; 63 volatile float gxlast = 0.0f; 64 65 volatile int justRead = 0; // This value is set when the timer was just read. It allows the main loop to react 66 67 // return the absolute value of the integer 68 int32_t abs(int32_t t) 69 { 70 return t<0 ? -t : t; 71 } 72 73 __attribute__((__interrupt__)) void Fabric_IRQHandler() { 74 NVIC_ClearPendingIRQ( Fabric_IRQn ); 75 I2C_isr(&imu); 76 } 77 78 void readIMU(i2c_instance_t* i2c, gyro* g, accelerometer* a, magnetometer* m) 79 { 80 L3G4200D_readGyro ( i2c, g ); 81 LSM303_readAccelerometer ( i2c, a ); 82 LSM303_readMagnetometer ( i2c, m ); 83 } 84 85 __attribute__((__interrupt__)) void Timer1_IRQHandler( void ) 86 { 87 MSS_TIM1_clear_irq(); 88 static int gxprev = 0; 89 90 // calculate the time value since the speed was last recorded. 91 static uint32_t last_time = 0xFFFFFFFF; // initialize at this, because it is starting value 92 volatile uint32_t current_time = MSS_TIM2_get_current_value(); 93 volatile float current = last_time - current_time; 94 95 last_time = current_time; 96 97 volatile float gx = ((float)((g.x + gxprev + 2*GYRO_OFFSET)/2)) * 2000.0f / (2 << 14) * current * 1.0e-8f; 98 degreesSpunSinceLast += gx; 99 integrated += gx; 100 101 // gxlast = gx; 102 gxprev = g.x; 103 104 justRead = 1; 105 } 106 107 108 109 110 int main() 111 { 112 A4988_turnMotorOff(); 113 114 imu.irqn = Fabric_IRQn; 115 I2C_init( &imu, 0x40050200, 0x00, I2C_PCLK_DIV_256 ); 116 117 L3G4200D_init( &imu ); 118 L3G4200D_writeReg( &imu, L3G4200D_CTRL_REG4, 0x20 ); // enable 2000 dps mode 119 LSM303_init( &imu ); 120 121 // INITIALIZE TIMER LAST, JUST BEFORE MAIN LOOP 122 MSS_TIM2_init(MSS_TIMER_PERIODIC_MODE); 123 MSS_TIM2_load_immediate(0xFFFFFFFF); 124 125 MSS_TIM1_init(MSS_TIMER_PERIODIC_MODE); 126 MSS_TIM1_load_background(0.001e8); // Once per millisecond 127 MSS_TIM1_enable_irq(); 128 MSS_TIM1_clear_irq(); 129 130 131 L3G4200D_readGyro_x( &imu, &g ); 132 133 MSS_TIM2_start(); 134 MSS_TIM1_start(); 135 136 // Create the PetPipe Object 137 // Create in rest state (0) and a Riemann sum quadrature method 138 //PetPipe *pipe = PetPipeMake(2,NULL); 139 140 uint32_t last_gyro_read_time = 0xFFFFFFFF; 141 uint32_t current_time; 142 143 144 int32_t cur_state = WAIT; 145 146 // indicates how far 'ahead' or 'behind' the weight is from the ideal 147 int32_t stepsOffset = 0; 148 int32_t timer_reading = 0; 149 150 while(1) 151 { 152 // if the gyro has not been read in a while, read the gyro 153 current_time = MSS_TIM2_get_current_value(); 154 if(last_gyro_read_time - current_time > GYRO_READ_FREQUENCY) 155 { 156 // this will set the global variable g, but only set x 157 // because it takes too long to set the others when we never use them. 158 L3G4200D_readGyro_x( &imu, &g ); 159 last_gyro_read_time = current_time; 160 } 161 162 // Switch for the current state the pipe is in 163 switch(cur_state) 164 { 165 /* WAIT: 166 // it basically sits in this state while it is home. When it is in 167 // WAIT it can rest from all its troubles and sorrows. I would imagine 168 // the pipe feels bullied all the time. Everybody pushing him around 169 // just to see if he will get back up and do it again. . .you know what, 170 // this little pipe is a hero (sob), even though his existence may seem 171 // pointless, he is an inspiration to us all! 172 // but back to the point, the pipe will be in WAIT until he starts moving. 173 // *When we get some time to reactor, we may look into removing this step 174 // and just having it start in the rolling away state. 175 // 176 // Enters from: INITIAL_STATE, 177 // BRAKE - when the pipe is in the home position and 178 // not moving much 179 // exits to: ROLLING_AWAY - when it starts to move 180 */ 181 case WAIT: 182 A4988_turnMotorOff(); 183 if( abs(degreesSpunSinceLast) > 2.0 184 || abs(integrated) > HOME_THRESHOLD_FAR ) 185 cur_state = ROLLING_AWAY; 186 break; 187 188 /* ROLLING_AWAY: 189 // while it is rolling away the weight will be tracked and the distance 190 // rolled will be tracked. We will assume that the pipe may roll pretty 191 // quick in this state (who doesn't want to freaking roll a pipe as fast 192 // as they can!). In other works, we may purposely slip the stepper motor 193 // in order to keep up with the roll. All that matters in this state is 194 // that we keep track of how far we have travel and the pendulum weight 195 // should stay perpendicular to the ground. 196 // 197 // Enter from: WAIT - when the pipe starts rolling and needs to be in active 198 // tracking 199 // exits to: PUSH_BACK_TRANSITION - when the speed drops bellow the threshold 200 */ 201 case ROLLING_AWAY: 202 //A4988_turnMotorOff(); 203 // tracking code 204 //if( abs(degreesSpunSinceLast) > 2.0 ) 205 //{ 206 //int turns = degreesSpunSinceLast / 1.8; 207 208 //A4988_rotRel(turns, ROLLING_AWAY_MOTOR_DELAY); 209 210 //__asm("cpsid i"); 211 //degreesSpunSinceLast -= 1.8 * turns; 212 //__asm("cpsie i"); 213 //} 214 215 // exits state when leaving speed is too low 216 if( abs(g.x) < LEAVING_SPEED_MIN) 217 { 218 // only start to return if it is far enough from 'home' to make 219 // it worth the effort 220 if(abs(integrated) < HOME_THRESHOLD_FAR) 221 cur_state = WAIT; 222 else 223 cur_state = PUSH_BACK_TRANSITION; 224 } 225 break; 226 227 /* PUSH_BACK_TRANSITION: 228 // This state helps to monitor the speed the pipe is rolling. 229 // it will enter this state when it needs to speed up. 230 // when speeding up, it will rotate the motor 90 degrees (50 steps) 231 // and will enter a tracking phase (TRACK_SPEEDUP). When it is going 232 // fast enough, it will return to this state, which will transfer 233 // control to the TRACK_RETURN state, which just rolls. 234 // 235 // enters from: ROLLING_AWAY - when it is going slow enough to start return 236 // TRACK_RETURN - when it is going too slow and wants to speed up 237 // TRACK_SPEEDUP - when it is going fast enough. 238 // exits to: TRACK_RETURN - if it is going fast enough, it will go to TRACK_RETURN 239 // TRACK_SPEEDUP - if it needs to speedup, rotates motor and goes here. 240 */ 241 case PUSH_BACK_TRANSITION: 242 //A4988_turnMotorOff(); 243 // if it entered this state in order to speed up 244 if( abs(g.x) < RETURN_SPEED_LOWER ) 245 { 246 A4988_turnMotorOn(); 247 248 // set the reading so it will know how long it has been 249 // in that state 250 timer_reading = current_time; 251 252 // turn the motor in the direction it need to speed up in. 253 // (the direction to get back to the home area) 254 if(integrated < 0.0) 255 stepsOffset = SPEED_UP_STEPS; 256 else 257 stepsOffset = -SPEED_UP_STEPS; 258 259 A4988_rotRel(stepsOffset, ROLLING_BACK_MOTOR_DELAY); 260 261 cur_state = TRACK_SPEEDUP; 262 } 263 else 264 { 265 A4988_turnMotorOff(); 266 // it must be going fast enough, so it will go to TRACK_ROLLING 267 cur_state = TRACK_RETURN; 268 } 269 break; 270 271 /* TRACK_SPEEDUP 272 // Tracks the weight as the roll increases in speed. If, while it is rotating, 273 // it discovers it has arrived at the home area, it will exit and enter the 274 // BRAKING state 275 // 276 // Enters from: PUSH_BACK_TRANSITION - after it adjusts the weight, this watches 277 // the speed 278 // Exits to: PUSH_BACK_TRANSITION - when it is going fast enough 279 // BRAKE - if while speeding up it reaches home 280 */ 281 case TRACK_SPEEDUP: 282 //A4988_turnMotorOn(); 283 // tracking phase 284 if( abs(degreesSpunSinceLast) > 2.0 ) 285 { 286 int turns = degreesSpunSinceLast / 1.8; 287 288 A4988_rotRel(turns, ROLLING_BACK_MOTOR_DELAY); 289 290 __asm("cpsid i"); 291 degreesSpunSinceLast -= 1.8 * turns; 292 __asm("cpsie i"); 293 } 294 // this means it is close enough to home for the pipe to feel 'safe' 295 if(abs(integrated) < HOME_THRESHOLD_CLOSE) 296 { 297 A4988_rotRel(-stepsOffset, ROLLING_BACK_MOTOR_DELAY); 298 stepsOffset = 0; 299 cur_state = BRAKING; 300 } 301 // check if it is going fast enough 302 else if(abs(g.x) > RETURN_SPEED_UPPER) 303 { 304 A4988_turnMotorOff(); 305 stepsOffset = 0; 306 cur_state = PUSH_BACK_TRANSITION; 307 } 308 // check to see if it has been in this state continuously too long 309 else if(timer_reading - current_time > SPEEDUP_TIMEOUT) 310 { 311 A4988_turnMotorOff(); 312 timer_reading = current_time; 313 cur_state = TRACK_SPEEDUP_TIMEOUT; 314 } 315 316 // otherwise, it should retain the current state of speeding up 317 break; 318 319 /* TRACK_RETURN: 320 // tracks the weight so it should stay perpendicular to the ground 321 // If it gets close enough to home it will enter the BRAKING state. 322 // If it gets too slow it will go into the state to speed it up. 323 // basically, this is tracking the weight and waiting for the pipe 324 // to move. Think of it like a parent which is waiting for their 325 // kid to move out. If they are going too slow they give their kid 326 // a push, if they are close enough, they call it good and don't help 327 // the kid anymore in life. 328 // 329 // Enters from: PUSH_BACK_TRANSITION - when it is going fast enough 330 // Exits to: PUSH_BACK_TRANSITION - if it is going too slow 331 // BRAKING - if it is in the home area 332 */ 333 case TRACK_RETURN: 334 A4988_turnMotorOff(); 335 // if it is close enough to home, start breaking 336 if(abs(integrated) < HOME_THRESHOLD_CLOSE) 337 { 338 cur_state = BRAKING; 339 } 340 else if(abs(g.x) < RETURN_SPEED_LOWER) // if it is going too slow 341 { 342 cur_state = PUSH_BACK_TRANSITION; 343 } 344 else // otherwise, track the weight 345 { 346 if( abs(degreesSpunSinceLast) > 2.0 ) 347 { 348 int turns = degreesSpunSinceLast / 1.8; 349 350 //A4988_rotRel(turns, ROLLING_BACK_MOTOR_DELAY); 351 352 __asm("cpsid i"); 353 degreesSpunSinceLast -= 1.8 * turns; 354 __asm("cpsie i"); 355 } 356 } 357 break; 358 359 /* BRAKING: 360 // This code is supposed to help the pipe slow down so it can stop in 361 // the home area. It tries to be the opposite of the speed up phase. 362 // It throws the weight in the opposite direction as it is traveling. 363 // After it slows down a bit it will consider itself stopped. It will 364 // return back to waiting. If it did not slow down fast enough to be 365 // inside the home area, then from the WAIT state it will go back through 366 // the states and will eventually find itself at home. 367 // 368 // I thought it would be cool if it slowed down faster if it was going fast, 369 // and when it did not have as much to slow down, it could change the 370 // weight to not be as high. This code is not needed. 371 // 372 // Enters From: TRACK_SPEEDUP - if it finds itself in the home area 373 // TRACK_RETURN - if it finds itself in the home area 374 // Exits to: WAIT - when it thinks it has stopped the pipe enough 375 */ 376 case BRAKING: 377 A4988_turnMotorOn(); 378 if( abs(degreesSpunSinceLast) > 2.0 ) 379 { 380 int turns = degreesSpunSinceLast / 1.8; 381 382 A4988_rotRel(turns, ROLLING_BACK_MOTOR_DELAY); 383 384 __asm("cpsid i"); 385 degreesSpunSinceLast -= 1.8 * turns; 386 __asm("cpsie i"); 387 } 388 389 if( abs(g.x) < BRAKING_MINIMUM) // if it is slow enough to be considered good 390 { 391 A4988_rotRel(-stepsOffset, ROLLING_BACK_MOTOR_DELAY); 392 A4988_turnMotorOff(); 393 stepsOffset = 0; 394 cur_state = WAIT; 395 } 396 else if( stepsOffset == 0 ) 397 { 398 // turn the motor in the direction it need to speed up in. 399 // (the direction to get back to the home area) 400 if(g.x < 0.0) 401 stepsOffset = SPEED_UP_STEPS; 402 else 403 stepsOffset = -SPEED_UP_STEPS; 404 405 A4988_rotRel(stepsOffset, ROLLING_BACK_MOTOR_DELAY); 406 407 } 408 break; 409 410 /* TRACK_SPEEDUP_TIMEOUT: 411 // This state happens when it has been speeding up for a long time. 412 // when in this state is just has to wait for some time before exiting. 413 // The hope is that it will have the motor drop to be perpendicular to 414 // the ground, so the speedup phase can have a fresh start. 415 // NOTE: it will return to TRACK_RETURN. If it is going very slow 416 // (which is expected) it will then move back into PUSHBACK_TRANSITION. 417 // if it somehow magically found itself in the home state it will figure 418 // that out at that time. 419 // 420 // ENTERS FROM: TRACK_SPEEDUP - after it has spent a long time in that 421 // section continuously 422 // EXITS TO: TRACK_RETURN - after it has 'cooled down' in this state 423 */ 424 case TRACK_SPEEDUP_TIMEOUT: 425 if(timer_reading - current_time > SPEEDUP_TIMEOUT_COOLDOWN) 426 cur_state = TRACK_RETURN; 427 break; 428 429 // DEFAULT 430 default: 431 cur_state = WAIT; 432 break; 433 } 434 435 } 436 }