Stap 9: Code--MCU toepassing.
{{{ <p>#include "mcu_api.h"<br>#include "mcu_errno.h"</p><p>#define INPUT 0 #define OUTPUT 1</p><p>int echoPin = 128; //2 int trigPin = 13; //5 int blinkPin = 40; //13 int leftMotor = 0; //pwm3 int rightMotor = 2; //pwm6 unsigned long startT, endT, elapTime, stopTime, tmpTime1; unsigned long counter = 1;</p><p>void pulseIn2(unsigned int state, int timeout) { int count = timeout; while (gpio_read(echoPin) != state && count > 0) count--; }</p><p>void turnLeft(int leftMotor, int rightMotor, int duration) { pwm_configure(leftMotor, 2000000, 20000000); pwm_configure(rightMotor, 2000000, 20000000); pwm_enable(leftMotor); pwm_enable(rightMotor); mcu_sleep(duration); }</p><p>void turnRight(int leftMotor, int rightMotor, int duration) {</p><p> pwm_configure(leftMotor, 700000, 20000000); pwm_configure(rightMotor, 700000, 20000000); pwm_enable(leftMotor); pwm_enable(rightMotor); mcu_sleep(duration); }</p><p>void go(int leftMotor, int rightMotor) {</p><p> gpio_write(blinkPin, 0);</p><p> pwm_configure(leftMotor, 700000, 20000000); pwm_configure(rightMotor, 2000000, 20000000); pwm_enable(leftMotor); pwm_enable(rightMotor);</p><p>}</p><p>void reverse(int leftMotor, int rightMotor, int duration) { gpio_write(blinkPin, 1);</p><p> pwm_configure(leftMotor, 2000000, 20000000); pwm_configure(rightMotor, 700000, 20000000); pwm_enable(leftMotor); pwm_enable(rightMotor); mcu_sleep(duration); }</p><p>void stopMotor(int leftMotor, int rightMotor) { // host_send((unsigned char*)"Stopped\n",8);</p><p> pwm_disable(leftMotor); pwm_disable(rightMotor); }</p><p>void incrStopTime(int reset) { if (tmpTime1 > 0) { stopTime += (time_ms() - tmpTime1); if (reset) tmpTime1 = 0; else tmpTime1=time_ms(); }; }</p><p>void mcu_main() { unsigned int runMotor = 1; unsigned char btInput = 0; char buf[64]; int len; unsigned long distTraveled; /* your configuration code starts here */ startT = time_ms(); elapTime = time_ms(); stopTime = 0;</p><p> gpio_setup(echoPin, INPUT); //in gpio_setup(trigPin, OUTPUT); //out gpio_setup(blinkPin, OUTPUT); //out</p><p> while (1) { mcu_sleep(50); btInput = 0; len = host_receive((unsigned char *) buf, 64); if (len > 0) { btInput = (char) buf[0]; switch (btInput) { case 'S': gpio_write(blinkPin, 1); stopMotor(leftMotor, rightMotor); tmpTime1 = time_ms(); runMotor = 0; break;</p><p> case 'A': gpio_write(blinkPin, 1); go(leftMotor, rightMotor); runMotor = 1; incrStopTime(1); break;</p><p> case 'L': gpio_write(blinkPin, 1); turnLeft(leftMotor, rightMotor, 95); go(leftMotor, rightMotor); runMotor = 1; incrStopTime(1);</p><p> break; case 'R': gpio_write(blinkPin, 1); // reverse(1000); // stopMotor(); turnRight(leftMotor, rightMotor, 95); go(leftMotor, rightMotor); runMotor = 1; incrStopTime(1);</p><p> break; case 'B': gpio_write(blinkPin, 1); turnLeft(leftMotor, rightMotor, 95); turnLeft(leftMotor, rightMotor, 95); go(leftMotor, rightMotor); runMotor = 1; incrStopTime(1); break; default: break; } } if (runMotor) { gpio_write(trigPin, 0); gpio_write(trigPin, 1);</p><p> gpio_write(trigPin, 0);</p><p> pulseIn2(1, 10000); long startTime = time_us();</p><p> pulseIn2(0, 10000); long endTime = time_us(); int duration = endTime - startTime;</p><p> int distance = duration / 2 / 29; //29.1?</p><p> if ((distance != 0) && (distance < 15)) { reverse(leftMotor, rightMotor, 95); stopMotor(leftMotor, rightMotor); turnLeft(leftMotor, rightMotor, 95); go(leftMotor, rightMotor); } else { go(leftMotor, rightMotor); } } endT = time_ms(); if ((endT - startT) > 10000) { incrStopTime(0); startT = time_ms(); distTraveled = (endT - elapTime - stopTime) * 12 / 60000; // approximate feed driven based to static RPM of servos. }; len = mcu_snprintf(buf, 16, "%d\n", distTraveled); host_send((unsigned char*) buf, len); } }</p> }}}