Diego Rivera / Marvino mbot

Demo program for the Marvino robot from Fortito (on board processor)

Dependencies:   mbed-src

main.cpp@0:3c55e8a81dfb, 2016-02-02 (annotated)

Committer:
diegorivera42
Date:
Tue Feb 02 12:04:37 2016 +0000
Revision:
0:3c55e8a81dfb
Demo version 1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
diegorivera42 0:3c55e8a81dfb 1 #include "mbed.h"
diegorivera42 0:3c55e8a81dfb 2
diegorivera42 0:3c55e8a81dfb 3 Timer t1;
diegorivera42 0:3c55e8a81dfb 4 Timer t2;
diegorivera42 0:3c55e8a81dfb 5
diegorivera42 0:3c55e8a81dfb 6 DigitalOut myled(LED1);
diegorivera42 0:3c55e8a81dfb 7 I2C i2c(p9, p10);
diegorivera42 0:3c55e8a81dfb 8 AnalogIn light(p18);
diegorivera42 0:3c55e8a81dfb 9 AnalogIn linel(p19);
diegorivera42 0:3c55e8a81dfb 10 AnalogIn liner(p20);
diegorivera42 0:3c55e8a81dfb 11 PwmOut eyes(p21);
diegorivera42 0:3c55e8a81dfb 12 PwmOut motlspd(p22);
diegorivera42 0:3c55e8a81dfb 13 PwmOut motrspd(p23);
diegorivera42 0:3c55e8a81dfb 14 DigitalOut motldir(p24);
diegorivera42 0:3c55e8a81dfb 15 DigitalOut motrdir(p25);
diegorivera42 0:3c55e8a81dfb 16 DigitalOut speaker(p26);
diegorivera42 0:3c55e8a81dfb 17 DigitalIn armr(p27);
diegorivera42 0:3c55e8a81dfb 18 DigitalIn armrf(p28);
diegorivera42 0:3c55e8a81dfb 19 DigitalIn arml(p29);
diegorivera42 0:3c55e8a81dfb 20 DigitalIn armlf(p30);
diegorivera42 0:3c55e8a81dfb 21
diegorivera42 0:3c55e8a81dfb 22 int HatLightAddr = 0x46; // PCA8575 16 bit IO
diegorivera42 0:3c55e8a81dfb 23 char HatLightData[2]; // 16 bit data
diegorivera42 0:3c55e8a81dfb 24 float PWMFreq = 5000; // PWM Frequency in Hz
diegorivera42 0:3c55e8a81dfb 25 float PWMPeriod = 1 / PWMFreq; // On the mbed this is global for all PWM output pins
diegorivera42 0:3c55e8a81dfb 26 float MotLeftAdj = 1; // Left motor speed adjust
diegorivera42 0:3c55e8a81dfb 27 float MotRightAdj = 1; // Right motor speed adjust
diegorivera42 0:3c55e8a81dfb 28 float vcc = 3.3; // mbed supply voltage
diegorivera42 0:3c55e8a81dfb 29 float ForwardFull = 1; // motor forward full speed
diegorivera42 0:3c55e8a81dfb 30 float ForwardNorm = ForwardFull * 0.5; // forward normal speed
diegorivera42 0:3c55e8a81dfb 31 float ForwardSlow = ForwardNorm * 0.5; // forward half normal speed
diegorivera42 0:3c55e8a81dfb 32 float ReverseNorm = 0-ForwardNorm; // reverse normal speed
diegorivera42 0:3c55e8a81dfb 33 float ReverseFull = 0-ForwardFull; // motor reverse full speed
diegorivera42 0:3c55e8a81dfb 34 float ReverseSlow = 0-ForwardSlow; // reverse half normal speed
diegorivera42 0:3c55e8a81dfb 35 float Stop = 0.0; // stop
diegorivera42 0:3c55e8a81dfb 36 float SensorTimeOut1 = 10; // Nothing happening to Arms or Line Following in seconds (change to line following)
diegorivera42 0:3c55e8a81dfb 37 float SensorTimeOut2 = 10; // Nothing happening to Light Following in seconds (stuck? so reverse turn)
diegorivera42 0:3c55e8a81dfb 38 int LightChange = 0; // used to determine change in light following
diegorivera42 0:3c55e8a81dfb 39 int LineFollow = 0; // used to determine continous run of line following
diegorivera42 0:3c55e8a81dfb 40 int LineBright = 1; // voltage that determines line brightness
diegorivera42 0:3c55e8a81dfb 41
diegorivera42 0:3c55e8a81dfb 42 // MotorLeft expects -1 to 1 float for speed and direction
diegorivera42 0:3c55e8a81dfb 43 void MotorLeft(float sp) {
diegorivera42 0:3c55e8a81dfb 44 if (sp > 0) {
diegorivera42 0:3c55e8a81dfb 45 motldir = 1;
diegorivera42 0:3c55e8a81dfb 46 }
diegorivera42 0:3c55e8a81dfb 47 else {
diegorivera42 0:3c55e8a81dfb 48 motldir = 0;
diegorivera42 0:3c55e8a81dfb 49 sp = 0 - sp;
diegorivera42 0:3c55e8a81dfb 50 }
diegorivera42 0:3c55e8a81dfb 51 motlspd.pulsewidth(sp * PWMPeriod * MotLeftAdj); // left motor a bit fast so slow it down
diegorivera42 0:3c55e8a81dfb 52 }
diegorivera42 0:3c55e8a81dfb 53
diegorivera42 0:3c55e8a81dfb 54 // MotorRight expects -1 to 1 float for speed and direction
diegorivera42 0:3c55e8a81dfb 55 void MotorRight(float sp) {
diegorivera42 0:3c55e8a81dfb 56 if (sp > 0) {
diegorivera42 0:3c55e8a81dfb 57 motrdir = 1;
diegorivera42 0:3c55e8a81dfb 58 }
diegorivera42 0:3c55e8a81dfb 59 else {
diegorivera42 0:3c55e8a81dfb 60 motrdir = 0;
diegorivera42 0:3c55e8a81dfb 61 sp = 0 - sp;
diegorivera42 0:3c55e8a81dfb 62 }
diegorivera42 0:3c55e8a81dfb 63 motrspd.pulsewidth(sp * PWMPeriod * MotRightAdj);
diegorivera42 0:3c55e8a81dfb 64 }
diegorivera42 0:3c55e8a81dfb 65
diegorivera42 0:3c55e8a81dfb 66 // EyesBright expects 0 to 1 float for brrightness
diegorivera42 0:3c55e8a81dfb 67 void EyesBright(float sp) {
diegorivera42 0:3c55e8a81dfb 68 eyes.pulsewidth(sp * PWMPeriod);
diegorivera42 0:3c55e8a81dfb 69 }
diegorivera42 0:3c55e8a81dfb 70
diegorivera42 0:3c55e8a81dfb 71 void HatLights(int d) {
diegorivera42 0:3c55e8a81dfb 72 d=d ^ 0xffff;
diegorivera42 0:3c55e8a81dfb 73 HatLightData[0]=(d);
diegorivera42 0:3c55e8a81dfb 74 HatLightData[1]=(d >>8);
diegorivera42 0:3c55e8a81dfb 75 i2c.write( HatLightAddr, HatLightData, 2 );
diegorivera42 0:3c55e8a81dfb 76 }
diegorivera42 0:3c55e8a81dfb 77
diegorivera42 0:3c55e8a81dfb 78 unsigned int m_z=12434,m_w=33254;
diegorivera42 0:3c55e8a81dfb 79
diegorivera42 0:3c55e8a81dfb 80 unsigned int rnd() {
diegorivera42 0:3c55e8a81dfb 81 m_z = 36969 * (m_z & 65535) + (m_z >>16);
diegorivera42 0:3c55e8a81dfb 82 m_w = 18000 * (m_w & 65535) + (m_w >>16);
diegorivera42 0:3c55e8a81dfb 83 return ((m_z <<16) + m_w);
diegorivera42 0:3c55e8a81dfb 84 }
diegorivera42 0:3c55e8a81dfb 85
diegorivera42 0:3c55e8a81dfb 86 // Idea sets hat lights to a random pattern n times at period in seconds
diegorivera42 0:3c55e8a81dfb 87 void Idea(int n, float p) {
diegorivera42 0:3c55e8a81dfb 88 for (int i=0; i<n; i++) {
diegorivera42 0:3c55e8a81dfb 89 HatLights(rnd()%0x8000);
diegorivera42 0:3c55e8a81dfb 90 wait(p);
diegorivera42 0:3c55e8a81dfb 91 }
diegorivera42 0:3c55e8a81dfb 92 HatLights(0);
diegorivera42 0:3c55e8a81dfb 93 }
diegorivera42 0:3c55e8a81dfb 94
diegorivera42 0:3c55e8a81dfb 95 // Thinking rotates hat lights n times at period p in seconds
diegorivera42 0:3c55e8a81dfb 96 void Thinking(int n, float p) {
diegorivera42 0:3c55e8a81dfb 97 int r=1;
diegorivera42 0:3c55e8a81dfb 98 for (int i=0; i<n; i++) {
diegorivera42 0:3c55e8a81dfb 99 HatLights(r);
diegorivera42 0:3c55e8a81dfb 100 wait(p);
diegorivera42 0:3c55e8a81dfb 101 if (r == 0x8000) {
diegorivera42 0:3c55e8a81dfb 102 r=1;
diegorivera42 0:3c55e8a81dfb 103 }
diegorivera42 0:3c55e8a81dfb 104 else {
diegorivera42 0:3c55e8a81dfb 105 r=(r <<1);
diegorivera42 0:3c55e8a81dfb 106 }
diegorivera42 0:3c55e8a81dfb 107 }
diegorivera42 0:3c55e8a81dfb 108 HatLights(0);
diegorivera42 0:3c55e8a81dfb 109 }
diegorivera42 0:3c55e8a81dfb 110
diegorivera42 0:3c55e8a81dfb 111 // Beep sounder at frequency f (cycles/sec) for period p in seconds
diegorivera42 0:3c55e8a81dfb 112 void Beep(float f, float p) {
diegorivera42 0:3c55e8a81dfb 113 float t = 0.5/f;
diegorivera42 0:3c55e8a81dfb 114 for (int i=0; i<(f*p); i++) {
diegorivera42 0:3c55e8a81dfb 115 speaker = 1; // speaker high
diegorivera42 0:3c55e8a81dfb 116 wait(t);
diegorivera42 0:3c55e8a81dfb 117 speaker = 0; // speaker low
diegorivera42 0:3c55e8a81dfb 118 wait(t);
diegorivera42 0:3c55e8a81dfb 119 }
diegorivera42 0:3c55e8a81dfb 120 }
diegorivera42 0:3c55e8a81dfb 121
diegorivera42 0:3c55e8a81dfb 122 // FlashEyes flashes eyes n times at period p in seconds
diegorivera42 0:3c55e8a81dfb 123 void FlashEyes(int n, float p) {
diegorivera42 0:3c55e8a81dfb 124 for (int i=0; i<n; i++) {
diegorivera42 0:3c55e8a81dfb 125 EyesBright(1); // eyes full on
diegorivera42 0:3c55e8a81dfb 126 wait(p/2);
diegorivera42 0:3c55e8a81dfb 127 EyesBright(0); // eyes off
diegorivera42 0:3c55e8a81dfb 128 wait(p/2);
diegorivera42 0:3c55e8a81dfb 129 }
diegorivera42 0:3c55e8a81dfb 130 EyesBright(0.5); // back to half bright
diegorivera42 0:3c55e8a81dfb 131 }
diegorivera42 0:3c55e8a81dfb 132
diegorivera42 0:3c55e8a81dfb 133 void AvoidLeft() {
diegorivera42 0:3c55e8a81dfb 134 MotorLeft(Stop);
diegorivera42 0:3c55e8a81dfb 135 MotorRight(Stop); // stop
diegorivera42 0:3c55e8a81dfb 136 Thinking(16, 0.02); // rotate hat lights
diegorivera42 0:3c55e8a81dfb 137 MotorLeft(ReverseSlow);
diegorivera42 0:3c55e8a81dfb 138 MotorRight(ReverseNorm); // reverse turn back
diegorivera42 0:3c55e8a81dfb 139 FlashEyes(3, 0.1);
diegorivera42 0:3c55e8a81dfb 140 MotorLeft(ForwardNorm);
diegorivera42 0:3c55e8a81dfb 141 MotorRight(ForwardNorm); // forward
diegorivera42 0:3c55e8a81dfb 142 }
diegorivera42 0:3c55e8a81dfb 143
diegorivera42 0:3c55e8a81dfb 144 void AvoidRight() {
diegorivera42 0:3c55e8a81dfb 145 MotorLeft(Stop);
diegorivera42 0:3c55e8a81dfb 146 MotorRight(Stop); // stop
diegorivera42 0:3c55e8a81dfb 147 Thinking(16, 0.02); // rotate hat lights
diegorivera42 0:3c55e8a81dfb 148 MotorLeft(ReverseNorm);
diegorivera42 0:3c55e8a81dfb 149 MotorRight(ReverseSlow); // reverse turn back
diegorivera42 0:3c55e8a81dfb 150 FlashEyes(3, 0.1);
diegorivera42 0:3c55e8a81dfb 151 MotorLeft(ForwardNorm);
diegorivera42 0:3c55e8a81dfb 152 MotorRight(ForwardNorm); // forward
diegorivera42 0:3c55e8a81dfb 153 }
diegorivera42 0:3c55e8a81dfb 154
diegorivera42 0:3c55e8a81dfb 155 void AvoidLeftFront() {
diegorivera42 0:3c55e8a81dfb 156 MotorLeft(Stop);
diegorivera42 0:3c55e8a81dfb 157 MotorRight(Stop); // stop
diegorivera42 0:3c55e8a81dfb 158 Thinking(16, 0.02); // rotate hat lights
diegorivera42 0:3c55e8a81dfb 159 MotorLeft(ReverseNorm);
diegorivera42 0:3c55e8a81dfb 160 MotorRight(ReverseNorm); // reverse
diegorivera42 0:3c55e8a81dfb 161 FlashEyes(6, 0.1); // flash eyes
diegorivera42 0:3c55e8a81dfb 162 MotorRight(Stop);
diegorivera42 0:3c55e8a81dfb 163 FlashEyes(6, 0.1); // rotate
diegorivera42 0:3c55e8a81dfb 164 MotorLeft(ForwardNorm);
diegorivera42 0:3c55e8a81dfb 165 MotorRight(ForwardNorm); // forward
diegorivera42 0:3c55e8a81dfb 166 }
diegorivera42 0:3c55e8a81dfb 167
diegorivera42 0:3c55e8a81dfb 168 void AvoidRightFront() {
diegorivera42 0:3c55e8a81dfb 169 MotorLeft(Stop);
diegorivera42 0:3c55e8a81dfb 170 MotorRight(Stop); // stop
diegorivera42 0:3c55e8a81dfb 171 Thinking(16, 0.02); // rotate hat lights
diegorivera42 0:3c55e8a81dfb 172 MotorLeft(ReverseNorm);
diegorivera42 0:3c55e8a81dfb 173 MotorRight(ReverseNorm); // reverse
diegorivera42 0:3c55e8a81dfb 174 FlashEyes(6, 0.1); // flash eyes
diegorivera42 0:3c55e8a81dfb 175 MotorLeft(Stop);
diegorivera42 0:3c55e8a81dfb 176 FlashEyes(6, 0.1); // rotate
diegorivera42 0:3c55e8a81dfb 177 MotorLeft(ForwardNorm);
diegorivera42 0:3c55e8a81dfb 178 MotorRight(ForwardNorm); // forward
diegorivera42 0:3c55e8a81dfb 179 }
diegorivera42 0:3c55e8a81dfb 180
diegorivera42 0:3c55e8a81dfb 181 void AvoidHeadOn() {
diegorivera42 0:3c55e8a81dfb 182 MotorLeft(Stop);
diegorivera42 0:3c55e8a81dfb 183 MotorRight(Stop); // stop
diegorivera42 0:3c55e8a81dfb 184 Thinking(50, 0.02); // rotate hat lights
diegorivera42 0:3c55e8a81dfb 185 Idea(20, 0.02); // flash random hat lights
diegorivera42 0:3c55e8a81dfb 186 MotorLeft(ReverseNorm);
diegorivera42 0:3c55e8a81dfb 187 MotorRight(ReverseNorm); // reverse
diegorivera42 0:3c55e8a81dfb 188 FlashEyes(8, 0.1); // flash eyes
diegorivera42 0:3c55e8a81dfb 189 MotorLeft(ForwardNorm);
diegorivera42 0:3c55e8a81dfb 190 FlashEyes(8, 0.1); // rotate clockwise
diegorivera42 0:3c55e8a81dfb 191 MotorRight(ForwardNorm); // carry on straight
diegorivera42 0:3c55e8a81dfb 192 }
diegorivera42 0:3c55e8a81dfb 193
diegorivera42 0:3c55e8a81dfb 194 void NudgeLeft() {
diegorivera42 0:3c55e8a81dfb 195 MotorLeft(Stop); // stop left motor
diegorivera42 0:3c55e8a81dfb 196 wait(0.01); // wait a bit
diegorivera42 0:3c55e8a81dfb 197 MotorLeft(ForwardNorm); // carry on
diegorivera42 0:3c55e8a81dfb 198 }
diegorivera42 0:3c55e8a81dfb 199
diegorivera42 0:3c55e8a81dfb 200 void NudgeRight() {
diegorivera42 0:3c55e8a81dfb 201 MotorRight(Stop); // stop right motor
diegorivera42 0:3c55e8a81dfb 202 wait(0.01); // wait a bit
diegorivera42 0:3c55e8a81dfb 203 MotorRight(ForwardNorm); // carry on
diegorivera42 0:3c55e8a81dfb 204 }
diegorivera42 0:3c55e8a81dfb 205
diegorivera42 0:3c55e8a81dfb 206 int main() {
diegorivera42 0:3c55e8a81dfb 207 motlspd.period(PWMPeriod);
diegorivera42 0:3c55e8a81dfb 208 motrspd.period(PWMPeriod);
diegorivera42 0:3c55e8a81dfb 209 eyes.period(PWMPeriod);
diegorivera42 0:3c55e8a81dfb 210 EyesBright(0.5); // eyes half brightness
diegorivera42 0:3c55e8a81dfb 211 MotorLeft(ForwardNorm); // motor left forward
diegorivera42 0:3c55e8a81dfb 212 MotorRight(ForwardNorm); // motor right forward
diegorivera42 0:3c55e8a81dfb 213 t1.start();
diegorivera42 0:3c55e8a81dfb 214 while(1) {
diegorivera42 0:3c55e8a81dfb 215 if (arml == 0 && armr != 0) {
diegorivera42 0:3c55e8a81dfb 216 t1.reset();
diegorivera42 0:3c55e8a81dfb 217 AvoidLeft();
diegorivera42 0:3c55e8a81dfb 218 }
diegorivera42 0:3c55e8a81dfb 219 if (arml != 0 && armr == 0) {
diegorivera42 0:3c55e8a81dfb 220 t1.reset();
diegorivera42 0:3c55e8a81dfb 221 AvoidRight();
diegorivera42 0:3c55e8a81dfb 222 }
diegorivera42 0:3c55e8a81dfb 223 if (arml == 0 && armr == 0) {
diegorivera42 0:3c55e8a81dfb 224 t1.reset();
diegorivera42 0:3c55e8a81dfb 225 AvoidHeadOn();
diegorivera42 0:3c55e8a81dfb 226 }
diegorivera42 0:3c55e8a81dfb 227 if (armlf == 0 && armrf == 0) {
diegorivera42 0:3c55e8a81dfb 228 t1.reset();
diegorivera42 0:3c55e8a81dfb 229 AvoidHeadOn();
diegorivera42 0:3c55e8a81dfb 230 }
diegorivera42 0:3c55e8a81dfb 231 if (armlf == 0 && armrf != 0) {
diegorivera42 0:3c55e8a81dfb 232 t1.reset();
diegorivera42 0:3c55e8a81dfb 233 AvoidLeftFront();
diegorivera42 0:3c55e8a81dfb 234 }
diegorivera42 0:3c55e8a81dfb 235 if (armlf != 0 && armrf == 0) {
diegorivera42 0:3c55e8a81dfb 236 t1.reset();
diegorivera42 0:3c55e8a81dfb 237 AvoidRightFront();
diegorivera42 0:3c55e8a81dfb 238 }
diegorivera42 0:3c55e8a81dfb 239 if ((linel > (LineBright/vcc)) && (liner < (LineBright/vcc))) {
diegorivera42 0:3c55e8a81dfb 240 NudgeLeft();
diegorivera42 0:3c55e8a81dfb 241 }
diegorivera42 0:3c55e8a81dfb 242 if ((linel < (LineBright/vcc)) && (liner > (LineBright/vcc))) {
diegorivera42 0:3c55e8a81dfb 243 NudgeRight();
diegorivera42 0:3c55e8a81dfb 244 }
diegorivera42 0:3c55e8a81dfb 245 if (((linel < (LineBright/vcc)) && (liner < (LineBright/vcc))) || ((linel > (LineBright/vcc)) && (liner > (LineBright/vcc)))) {
diegorivera42 0:3c55e8a81dfb 246 LineFollow = 0;
diegorivera42 0:3c55e8a81dfb 247 }
diegorivera42 0:3c55e8a81dfb 248 else {
diegorivera42 0:3c55e8a81dfb 249 LineFollow++;
diegorivera42 0:3c55e8a81dfb 250 if (LineFollow > 10) {
diegorivera42 0:3c55e8a81dfb 251 t1.reset(); // only reset activity timer if consistant line following
diegorivera42 0:3c55e8a81dfb 252 }
diegorivera42 0:3c55e8a81dfb 253 }
diegorivera42 0:3c55e8a81dfb 254 if (t1.read() > SensorTimeOut1) { // if nothing else is happening then follow light
diegorivera42 0:3c55e8a81dfb 255 t2.start();
diegorivera42 0:3c55e8a81dfb 256 if (light > 0.7) { // more light to the left
diegorivera42 0:3c55e8a81dfb 257 if (LightChange == 0) {
diegorivera42 0:3c55e8a81dfb 258 LightChange = 1;
diegorivera42 0:3c55e8a81dfb 259 t2.reset();
diegorivera42 0:3c55e8a81dfb 260 Beep(4000,0.1);
diegorivera42 0:3c55e8a81dfb 261 }
diegorivera42 0:3c55e8a81dfb 262 NudgeLeft();
diegorivera42 0:3c55e8a81dfb 263 }
diegorivera42 0:3c55e8a81dfb 264 if (light < 0.3) { // more light to the right
diegorivera42 0:3c55e8a81dfb 265 if (LightChange == 1) {
diegorivera42 0:3c55e8a81dfb 266 LightChange = 0;
diegorivera42 0:3c55e8a81dfb 267 t2.reset();
diegorivera42 0:3c55e8a81dfb 268 Beep(1000,0.1);
diegorivera42 0:3c55e8a81dfb 269 }
diegorivera42 0:3c55e8a81dfb 270 NudgeRight();
diegorivera42 0:3c55e8a81dfb 271 }
diegorivera42 0:3c55e8a81dfb 272 }
diegorivera42 0:3c55e8a81dfb 273 else {
diegorivera42 0:3c55e8a81dfb 274 t2.reset();
diegorivera42 0:3c55e8a81dfb 275 t2.stop();
diegorivera42 0:3c55e8a81dfb 276 }
diegorivera42 0:3c55e8a81dfb 277 if (t2.read() > SensorTimeOut2) { // probably stuck
diegorivera42 0:3c55e8a81dfb 278 AvoidHeadOn();
diegorivera42 0:3c55e8a81dfb 279 t1.reset();
diegorivera42 0:3c55e8a81dfb 280 t2.reset();
diegorivera42 0:3c55e8a81dfb 281 t2.stop();
diegorivera42 0:3c55e8a81dfb 282 }
diegorivera42 0:3c55e8a81dfb 283 }
diegorivera42 0:3c55e8a81dfb 284 }