Matthias Praja / Mbed 2 deprecated BERTL14_MOTOR_SENSOR

Motor Sensor

Dependencies:   mbed

Fork of BERTL14_MOTOR_SENSOR by Wolfgang Raimann

main.cpp@1:9b2f397ca266, 2016-01-08 (annotated)

Committer:
Matthias_Praja
Date:
Fri Jan 08 16:00:48 2016 +0000
Revision:
1:9b2f397ca266
Parent:
0:9d98c838cffc 
Motor Sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Matthias_Praja 1:9b2f397ca266 1
Matthias_Praja 1:9b2f397ca266 2 // Die Reifen machen nach einem Reset genau eine Umdrehung (24 Impulse)
Matthias_Praja 1:9b2f397ca266 3 //***********************************/
bulme_rai 0:9d98c838cffc 4
bulme_rai 0:9d98c838cffc 5 #include "mbed.h"
bulme_rai 0:9d98c838cffc 6
bulme_rai 0:9d98c838cffc 7 PwmOut MotorL_EN(p34);
bulme_rai 0:9d98c838cffc 8 DigitalOut MotorL_FORWARD(P1_0);
bulme_rai 0:9d98c838cffc 9 DigitalOut MotorL_REVERSE(P1_1);
bulme_rai 0:9d98c838cffc 10
bulme_rai 0:9d98c838cffc 11 PwmOut MotorR_EN(p36);
bulme_rai 0:9d98c838cffc 12 DigitalOut MotorR_FORWARD(P1_3);
bulme_rai 0:9d98c838cffc 13 DigitalOut MotorR_REVERSE(P1_4);
bulme_rai 0:9d98c838cffc 14
bulme_rai 0:9d98c838cffc 15 InterruptIn MotorL_Sensor(P1_12);
bulme_rai 0:9d98c838cffc 16 InterruptIn MotorR_Sensor(P1_13);
bulme_rai 0:9d98c838cffc 17
bulme_rai 0:9d98c838cffc 18 DigitalOut Dio10(LED1);
bulme_rai 0:9d98c838cffc 19 DigitalOut Dio11(LED2);
bulme_rai 0:9d98c838cffc 20 DigitalOut Dio12(LED3);
bulme_rai 0:9d98c838cffc 21 DigitalOut Dio13(LED4);
bulme_rai 0:9d98c838cffc 22
bulme_rai 0:9d98c838cffc 23 volatile int cntMotorL_Sensor = 0;
bulme_rai 0:9d98c838cffc 24 volatile int cntMotorR_Sensor = 0;
bulme_rai 0:9d98c838cffc 25
bulme_rai 0:9d98c838cffc 26 void ISR_MotorL_Sensor(){
bulme_rai 0:9d98c838cffc 27 if(MotorL_FORWARD)
bulme_rai 0:9d98c838cffc 28 cntMotorL_Sensor++;
bulme_rai 0:9d98c838cffc 29 else
bulme_rai 0:9d98c838cffc 30 cntMotorL_Sensor--;
bulme_rai 0:9d98c838cffc 31 }
bulme_rai 0:9d98c838cffc 32 void ISR_MotorR_Sensor(){
bulme_rai 0:9d98c838cffc 33 if(MotorR_FORWARD)
bulme_rai 0:9d98c838cffc 34 cntMotorR_Sensor++;
bulme_rai 0:9d98c838cffc 35 else
bulme_rai 0:9d98c838cffc 36 cntMotorR_Sensor--;
bulme_rai 0:9d98c838cffc 37 }
bulme_rai 0:9d98c838cffc 38
bulme_rai 0:9d98c838cffc 39 int main() {
bulme_rai 0:9d98c838cffc 40 MotorL_Sensor.rise(&ISR_MotorL_Sensor);
bulme_rai 0:9d98c838cffc 41 MotorR_Sensor.rise(&ISR_MotorR_Sensor);
bulme_rai 0:9d98c838cffc 42 // Richtungen werden bestimmt
bulme_rai 0:9d98c838cffc 43 MotorL_FORWARD = MotorR_FORWARD = 1;
bulme_rai 0:9d98c838cffc 44 // Periodendauer wird eingestellt
Matthias_Praja 1:9b2f397ca266 45 MotorL_EN.period_ms(20);
Matthias_Praja 1:9b2f397ca266 46 MotorR_EN.period_ms(20);
Matthias_Praja 1:9b2f397ca266 47 MotorL_EN = 1.0f;
Matthias_Praja 1:9b2f397ca266 48 MotorR_EN = 1.0f;
bulme_rai 0:9d98c838cffc 49
bulme_rai 0:9d98c838cffc 50 while(1) {
Matthias_Praja 1:9b2f397ca266 51 if(cntMotorL_Sensor < 30){
Matthias_Praja 1:9b2f397ca266 52 if(MotorL_EN != 5.0f) MotorL_EN = 5.0f;
bulme_rai 0:9d98c838cffc 53 MotorL_FORWARD = 1;
bulme_rai 0:9d98c838cffc 54 MotorL_REVERSE = 0;
Matthias_Praja 1:9b2f397ca266 55 } else if(cntMotorL_Sensor > 30){
Matthias_Praja 1:9b2f397ca266 56 if(MotorL_EN != 5.0f) MotorL_EN = 5.0f;
bulme_rai 0:9d98c838cffc 57 MotorL_FORWARD = 0;
bulme_rai 0:9d98c838cffc 58 MotorL_REVERSE = 1;
Matthias_Praja 1:9b2f397ca266 59 } else if(MotorL_EN != 2.0f){
Matthias_Praja 1:9b2f397ca266 60 MotorL_EN = 2.0f;
bulme_rai 0:9d98c838cffc 61 }
Matthias_Praja 1:9b2f397ca266 62 if(cntMotorR_Sensor < 30){
Matthias_Praja 1:9b2f397ca266 63 if(MotorR_EN != 5.0f) MotorR_EN = 5.0f;
bulme_rai 0:9d98c838cffc 64 MotorR_FORWARD = 1;
bulme_rai 0:9d98c838cffc 65 MotorR_REVERSE = 0;
Matthias_Praja 1:9b2f397ca266 66 } else if(cntMotorR_Sensor > 30){
Matthias_Praja 1:9b2f397ca266 67 if(MotorR_EN != 5.0f) MotorR_EN = 5.0f;
bulme_rai 0:9d98c838cffc 68 MotorR_FORWARD = 0;
bulme_rai 0:9d98c838cffc 69 MotorR_REVERSE = 1;
Matthias_Praja 1:9b2f397ca266 70 } else if(MotorR_EN != 2.0f){
Matthias_Praja 1:9b2f397ca266 71 MotorR_EN = 2.0f;
bulme_rai 0:9d98c838cffc 72 }
bulme_rai 0:9d98c838cffc 73 }
bulme_rai 0:9d98c838cffc 74 }