Line following bot using MAXREFDES89 and MAX32600MBED
Dependencies: MAX14871_Shield mbed
Diff: main.cpp
- Revision:
- 3:d268f6e06b7a
- Parent:
- 2:2c35ad38bf00
- Child:
- 4:eb6d6d25355c
diff -r 2c35ad38bf00 -r d268f6e06b7a main.cpp --- a/main.cpp Thu Dec 17 02:46:09 2015 +0000 +++ b/main.cpp Mon Dec 21 00:40:11 2015 +0000 @@ -54,11 +54,27 @@ const int32_t MAX_DUTY_CYCLE = 100; const int32_t MIN_DUTY_CYCLE = -100; + + /*************************************************** + | Control Type | KP | KI | KD | + |---------------|--------|------------|------------| + | P | 0.5Kc | 0 | 0 | + |--------------------------------------------------- + | PI | 0.45Kc | 1.2KpdT/Pc | 0 | + |---------------|--------|------------|------------| + | PD | 0.80Kc | 0 | KpPc/(8dT) | + |---------------|--------|------------|------------| + | PID | 0.60Kc | 2KpdT/Pc | KpPc/(8dT) | + ***************************************************/ + + //Kc = critical gain, gain with just P control at which system becomes marginally stable + //Pc = period of oscillation for previous scenario. + //set PID terms to 0 if not used/needed const int32_t KP = 16; const int32_t KI = 0; const int32_t KD = 0; - const int32_t TARGET_DUTY_CYCLE = 50; //starts bot off at 80% duty cycle + const int32_t TARGET_DUTY_CYCLE = 50; //raw sensor data uint8_t ir_val = 0; @@ -90,6 +106,9 @@ const uint8_t ERROR_RT1 = 0x0F; //b00001111 const uint8_t ERROR_RT2 = 0x1F; //b00011111 + //Lost bot error signal + const uint8_t ERROR_OFF_TRACK = 0xFF; //b11111111 + int32_t current_error = 0; int32_t previous_error = 0; @@ -198,10 +217,14 @@ case(ERROR_RT2): current_error = -7; break; + + case(ERROR_OFF_TRACK): + //do circles cause I am lost + current_error = 7; + break; default: - //do circles because I am a lost bot - current_error = 7; + current_error = previous_error; break; } @@ -228,13 +251,13 @@ previous_error = current_error; //get new duty cycle for right motor - r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + KI*integral + KD*derivative)); + r_duty_cycle = (TARGET_DUTY_CYCLE - (KP*current_error + (KI*integral) + KD*derivative)); //clamp to limits r_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, r_duty_cycle ); //get new duty cycle for left motor - l_duty_cycle = (TARGET_DUTY_CYCLE + (KP*current_error + KI*integral + KD*derivative)); + l_duty_cycle = (TARGET_DUTY_CYCLE + (KP*current_error + (KI*integral) + KD*derivative)); //clamp to limits l_duty_cycle = clamp_duty_cycle(MAX_DUTY_CYCLE, MIN_DUTY_CYCLE, l_duty_cycle );