Matthias Praja
/
BERTL14_MOTOR_SENSOR
Motor Sensor
Fork of BERTL14_MOTOR_SENSOR by
main.cpp
- Committer:
- Matthias_Praja
- Date:
- 2016-01-08
- Revision:
- 1:9b2f397ca266
- Parent:
- 0:9d98c838cffc
File content as of revision 1:9b2f397ca266:
// Die Reifen machen nach einem Reset genau eine Umdrehung (24 Impulse) //***********************************/ #include "mbed.h" PwmOut MotorL_EN(p34); DigitalOut MotorL_FORWARD(P1_0); DigitalOut MotorL_REVERSE(P1_1); PwmOut MotorR_EN(p36); DigitalOut MotorR_FORWARD(P1_3); DigitalOut MotorR_REVERSE(P1_4); InterruptIn MotorL_Sensor(P1_12); InterruptIn MotorR_Sensor(P1_13); DigitalOut Dio10(LED1); DigitalOut Dio11(LED2); DigitalOut Dio12(LED3); DigitalOut Dio13(LED4); volatile int cntMotorL_Sensor = 0; volatile int cntMotorR_Sensor = 0; void ISR_MotorL_Sensor(){ if(MotorL_FORWARD) cntMotorL_Sensor++; else cntMotorL_Sensor--; } void ISR_MotorR_Sensor(){ if(MotorR_FORWARD) cntMotorR_Sensor++; else cntMotorR_Sensor--; } int main() { MotorL_Sensor.rise(&ISR_MotorL_Sensor); MotorR_Sensor.rise(&ISR_MotorR_Sensor); // Richtungen werden bestimmt MotorL_FORWARD = MotorR_FORWARD = 1; // Periodendauer wird eingestellt MotorL_EN.period_ms(20); MotorR_EN.period_ms(20); MotorL_EN = 1.0f; MotorR_EN = 1.0f; while(1) { if(cntMotorL_Sensor < 30){ if(MotorL_EN != 5.0f) MotorL_EN = 5.0f; MotorL_FORWARD = 1; MotorL_REVERSE = 0; } else if(cntMotorL_Sensor > 30){ if(MotorL_EN != 5.0f) MotorL_EN = 5.0f; MotorL_FORWARD = 0; MotorL_REVERSE = 1; } else if(MotorL_EN != 2.0f){ MotorL_EN = 2.0f; } if(cntMotorR_Sensor < 30){ if(MotorR_EN != 5.0f) MotorR_EN = 5.0f; MotorR_FORWARD = 1; MotorR_REVERSE = 0; } else if(cntMotorR_Sensor > 30){ if(MotorR_EN != 5.0f) MotorR_EN = 5.0f; MotorR_FORWARD = 0; MotorR_REVERSE = 1; } else if(MotorR_EN != 2.0f){ MotorR_EN = 2.0f; } } }