Diego Rivera
Demo program for the Marvino robot from Fortito (on board processor)
Diff: main.cpp
- Revision:
- 0:3c55e8a81dfb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 02 12:04:37 2016 +0000
@@ -0,0 +1,284 @@
+#include "mbed.h"
+
+Timer t1;
+Timer t2;
+
+DigitalOut myled(LED1);
+I2C i2c(p9, p10);
+AnalogIn light(p18);
+AnalogIn linel(p19);
+AnalogIn liner(p20);
+PwmOut eyes(p21);
+PwmOut motlspd(p22);
+PwmOut motrspd(p23);
+DigitalOut motldir(p24);
+DigitalOut motrdir(p25);
+DigitalOut speaker(p26);
+DigitalIn armr(p27);
+DigitalIn armrf(p28);
+DigitalIn arml(p29);
+DigitalIn armlf(p30);
+
+int HatLightAddr = 0x46; // PCA8575 16 bit IO
+char HatLightData[2]; // 16 bit data
+float PWMFreq = 5000; // PWM Frequency in Hz
+float PWMPeriod = 1 / PWMFreq; // On the mbed this is global for all PWM output pins
+float MotLeftAdj = 1; // Left motor speed adjust
+float MotRightAdj = 1; // Right motor speed adjust
+float vcc = 3.3; // mbed supply voltage
+float ForwardFull = 1; // motor forward full speed
+float ForwardNorm = ForwardFull * 0.5; // forward normal speed
+float ForwardSlow = ForwardNorm * 0.5; // forward half normal speed
+float ReverseNorm = 0-ForwardNorm; // reverse normal speed
+float ReverseFull = 0-ForwardFull; // motor reverse full speed
+float ReverseSlow = 0-ForwardSlow; // reverse half normal speed
+float Stop = 0.0; // stop
+float SensorTimeOut1 = 10; // Nothing happening to Arms or Line Following in seconds (change to line following)
+float SensorTimeOut2 = 10; // Nothing happening to Light Following in seconds (stuck? so reverse turn)
+int LightChange = 0; // used to determine change in light following
+int LineFollow = 0; // used to determine continous run of line following
+int LineBright = 1; // voltage that determines line brightness
+
+// MotorLeft expects -1 to 1 float for speed and direction
+void MotorLeft(float sp) {
+ if (sp > 0) {
+ motldir = 1;
+ }
+ else {
+ motldir = 0;
+ sp = 0 - sp;
+ }
+ motlspd.pulsewidth(sp * PWMPeriod * MotLeftAdj); // left motor a bit fast so slow it down
+}
+
+// MotorRight expects -1 to 1 float for speed and direction
+void MotorRight(float sp) {
+ if (sp > 0) {
+ motrdir = 1;
+ }
+ else {
+ motrdir = 0;
+ sp = 0 - sp;
+ }
+ motrspd.pulsewidth(sp * PWMPeriod * MotRightAdj);
+}
+
+// EyesBright expects 0 to 1 float for brrightness
+void EyesBright(float sp) {
+ eyes.pulsewidth(sp * PWMPeriod);
+}
+
+void HatLights(int d) {
+ d=d ^ 0xffff;
+ HatLightData[0]=(d);
+ HatLightData[1]=(d >>8);
+ i2c.write( HatLightAddr, HatLightData, 2 );
+}
+
+unsigned int m_z=12434,m_w=33254;
+
+unsigned int rnd() {
+ m_z = 36969 * (m_z & 65535) + (m_z >>16);
+ m_w = 18000 * (m_w & 65535) + (m_w >>16);
+ return ((m_z <<16) + m_w);
+}
+
+// Idea sets hat lights to a random pattern n times at period in seconds
+void Idea(int n, float p) {
+ for (int i=0; i<n; i++) {
+ HatLights(rnd()%0x8000);
+ wait(p);
+ }
+ HatLights(0);
+}
+
+// Thinking rotates hat lights n times at period p in seconds
+void Thinking(int n, float p) {
+ int r=1;
+ for (int i=0; i<n; i++) {
+ HatLights(r);
+ wait(p);
+ if (r == 0x8000) {
+ r=1;
+ }
+ else {
+ r=(r <<1);
+ }
+ }
+ HatLights(0);
+}
+
+// Beep sounder at frequency f (cycles/sec) for period p in seconds
+void Beep(float f, float p) {
+ float t = 0.5/f;
+ for (int i=0; i<(f*p); i++) {
+ speaker = 1; // speaker high
+ wait(t);
+ speaker = 0; // speaker low
+ wait(t);
+ }
+}
+
+// FlashEyes flashes eyes n times at period p in seconds
+void FlashEyes(int n, float p) {
+ for (int i=0; i<n; i++) {
+ EyesBright(1); // eyes full on
+ wait(p/2);
+ EyesBright(0); // eyes off
+ wait(p/2);
+ }
+ EyesBright(0.5); // back to half bright
+}
+
+void AvoidLeft() {
+ MotorLeft(Stop);
+ MotorRight(Stop); // stop
+ Thinking(16, 0.02); // rotate hat lights
+ MotorLeft(ReverseSlow);
+ MotorRight(ReverseNorm); // reverse turn back
+ FlashEyes(3, 0.1);
+ MotorLeft(ForwardNorm);
+ MotorRight(ForwardNorm); // forward
+}
+
+void AvoidRight() {
+ MotorLeft(Stop);
+ MotorRight(Stop); // stop
+ Thinking(16, 0.02); // rotate hat lights
+ MotorLeft(ReverseNorm);
+ MotorRight(ReverseSlow); // reverse turn back
+ FlashEyes(3, 0.1);
+ MotorLeft(ForwardNorm);
+ MotorRight(ForwardNorm); // forward
+}
+
+void AvoidLeftFront() {
+ MotorLeft(Stop);
+ MotorRight(Stop); // stop
+ Thinking(16, 0.02); // rotate hat lights
+ MotorLeft(ReverseNorm);
+ MotorRight(ReverseNorm); // reverse
+ FlashEyes(6, 0.1); // flash eyes
+ MotorRight(Stop);
+ FlashEyes(6, 0.1); // rotate
+ MotorLeft(ForwardNorm);
+ MotorRight(ForwardNorm); // forward
+}
+
+void AvoidRightFront() {
+ MotorLeft(Stop);
+ MotorRight(Stop); // stop
+ Thinking(16, 0.02); // rotate hat lights
+ MotorLeft(ReverseNorm);
+ MotorRight(ReverseNorm); // reverse
+ FlashEyes(6, 0.1); // flash eyes
+ MotorLeft(Stop);
+ FlashEyes(6, 0.1); // rotate
+ MotorLeft(ForwardNorm);
+ MotorRight(ForwardNorm); // forward
+}
+
+void AvoidHeadOn() {
+ MotorLeft(Stop);
+ MotorRight(Stop); // stop
+ Thinking(50, 0.02); // rotate hat lights
+ Idea(20, 0.02); // flash random hat lights
+ MotorLeft(ReverseNorm);
+ MotorRight(ReverseNorm); // reverse
+ FlashEyes(8, 0.1); // flash eyes
+ MotorLeft(ForwardNorm);
+ FlashEyes(8, 0.1); // rotate clockwise
+ MotorRight(ForwardNorm); // carry on straight
+}
+
+void NudgeLeft() {
+ MotorLeft(Stop); // stop left motor
+ wait(0.01); // wait a bit
+ MotorLeft(ForwardNorm); // carry on
+}
+
+void NudgeRight() {
+ MotorRight(Stop); // stop right motor
+ wait(0.01); // wait a bit
+ MotorRight(ForwardNorm); // carry on
+}
+
+int main() {
+ motlspd.period(PWMPeriod);
+ motrspd.period(PWMPeriod);
+ eyes.period(PWMPeriod);
+ EyesBright(0.5); // eyes half brightness
+ MotorLeft(ForwardNorm); // motor left forward
+ MotorRight(ForwardNorm); // motor right forward
+ t1.start();
+ while(1) {
+ if (arml == 0 && armr != 0) {
+ t1.reset();
+ AvoidLeft();
+ }
+ if (arml != 0 && armr == 0) {
+ t1.reset();
+ AvoidRight();
+ }
+ if (arml == 0 && armr == 0) {
+ t1.reset();
+ AvoidHeadOn();
+ }
+ if (armlf == 0 && armrf == 0) {
+ t1.reset();
+ AvoidHeadOn();
+ }
+ if (armlf == 0 && armrf != 0) {
+ t1.reset();
+ AvoidLeftFront();
+ }
+ if (armlf != 0 && armrf == 0) {
+ t1.reset();
+ AvoidRightFront();
+ }
+ if ((linel > (LineBright/vcc)) && (liner < (LineBright/vcc))) {
+ NudgeLeft();
+ }
+ if ((linel < (LineBright/vcc)) && (liner > (LineBright/vcc))) {
+ NudgeRight();
+ }
+ if (((linel < (LineBright/vcc)) && (liner < (LineBright/vcc))) || ((linel > (LineBright/vcc)) && (liner > (LineBright/vcc)))) {
+ LineFollow = 0;
+ }
+ else {
+ LineFollow++;
+ if (LineFollow > 10) {
+ t1.reset(); // only reset activity timer if consistant line following
+ }
+ }
+ if (t1.read() > SensorTimeOut1) { // if nothing else is happening then follow light
+ t2.start();
+ if (light > 0.7) { // more light to the left
+ if (LightChange == 0) {
+ LightChange = 1;
+ t2.reset();
+ Beep(4000,0.1);
+ }
+ NudgeLeft();
+ }
+ if (light < 0.3) { // more light to the right
+ if (LightChange == 1) {
+ LightChange = 0;
+ t2.reset();
+ Beep(1000,0.1);
+ }
+ NudgeRight();
+ }
+ }
+ else {
+ t2.reset();
+ t2.stop();
+ }
+ if (t2.read() > SensorTimeOut2) { // probably stuck
+ AvoidHeadOn();
+ t1.reset();
+ t2.reset();
+ t2.stop();
+ }
+ }
+}