Allison Olin / LineFollower

things

Dependencies:   Motor

Fork of LineFollower by Bill Bonner

LineFollower.cpp@3:a5f6e29caddc, 2014-02-27 (annotated)

Committer:
alolin
Date:
Thu Feb 27 00:59:49 2014 +0000
Revision:
3:a5f6e29caddc
Parent:
2:7a4179249fa4 
No Change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
blu12758 0:2623af9e8ef3 1 #include "LineFollower.h"
blu12758 0:2623af9e8ef3 2 #include "mbed.h"
blu12758 0:2623af9e8ef3 3 #include <stdint.h>
blu12758 0:2623af9e8ef3 4
blu12758 0:2623af9e8ef3 5 /** Create a Line Follower interface for an IR Sensor Array
blu12758 0:2623af9e8ef3 6 *
blu12758 0:2623af9e8ef3 7 * @param ir1 IR Sensor 1
blu12758 0:2623af9e8ef3 8 * @param ir2 IR Sensor 2
blu12758 0:2623af9e8ef3 9 * @param ir3 IR Sensor 3
blu12758 0:2623af9e8ef3 10 * @param ir4 IR Sensor 4
blu12758 0:2623af9e8ef3 11 * @param ir5 IR Sensor 5
blu12758 0:2623af9e8ef3 12 * @param ir6 IR Sensor 6
blu12758 0:2623af9e8ef3 13 * @param ir7 IR Sensor 7
blu12758 0:2623af9e8ef3 14 * @param ir8 IR Sensor 8
blu12758 0:2623af9e8ef3 15 */
blu12758 0:2623af9e8ef3 16 LineFollower::Linefollower(DigitalIn ir1, DigitalIn ir2, DigitalIn ir3, DigitalIn ir4,
blu12758 0:2623af9e8ef3 17 DigitalIn ir5, DigitalIn ir6, DigitalIn ir7, DigitalIn ir8):
blu12758 0:2623af9e8ef3 18 _ir1(ir1), _ir2(ir2), _ir3(ir3), _ir4(ir4), _ir5(ir5), _ir6(ir6),
blu12758 0:2623af9e8ef3 19 _ir7(ir7), _ir8(ir8){
blu12758 0:2623af9e8ef3 20
blu12758 1:c319e24af8df 21 array[0] = _ir1;
blu12758 1:c319e24af8df 22 array[1] = _ir2;
blu12758 1:c319e24af8df 23 array[2] = _ir3;
blu12758 1:c319e24af8df 24 array[3] = _ir4;
blu12758 1:c319e24af8df 25 array[4] = _ir5;
blu12758 1:c319e24af8df 26 array[5] = _ir6;
blu12758 1:c319e24af8df 27 array[6] = _ir7;
blu12758 1:c319e24af8df 28 array[7] = _ir8;
blu12758 0:2623af9e8ef3 29 }
blu12758 0:2623af9e8ef3 30
blu12758 0:2623af9e8ef3 31
blu12758 0:2623af9e8ef3 32 /** Read the value of a LineFollower object
blu12758 0:2623af9e8ef3 33 *
blu12758 0:2623af9e8ef3 34 * @return The value of the Sensor
blu12758 0:2623af9e8ef3 35 */
blu12758 0:2623af9e8ef3 36 uint8_t LineFollower::read(){
blu12758 0:2623af9e8ef3 37 uint8_t binary = 0;
blu12758 1:c319e24af8df 38 int multi = 1;
blu12758 2:7a4179249fa4 39 for(int i=0; i<8; i++){
blu12758 1:c319e24af8df 40 binary += array[i]*multi;
blu12758 1:c319e24af8df 41 multi = multi*2;
blu12758 1:c319e24af8df 42 }
blu12758 1:c319e24af8df 43 return binary;
blu12758 1:c319e24af8df 44 }
blu12758 1:c319e24af8df 45
blu12758 1:c319e24af8df 46 /** Follow a line
blu12758 1:c319e24af8df 47 *
blu12758 1:c319e24af8df 48 * @param l left drive motor
blu12758 1:c319e24af8df 49 * @param r right drive motor
blu12758 1:c319e24af8df 50 */
blu12758 1:c319e24af8df 51 void followLine(Motor l, Motor r){
blu12758 1:c319e24af8df 52 int count = 0;
blu12758 1:c319e24af8df 53 for(int i = 0; i<8; i++){
blu12758 1:c319e24af8df 54 count += array[i];
blu12758 1:c319e24af8df 55 }
blu12758 1:c319e24af8df 56
blu12758 1:c319e24af8df 57 switch(count){
blu12758 1:c319e24af8df 58
blu12758 1:c319e24af8df 59 case 1: if(this->read() == 0b10000000){
blu12758 2:7a4179249fa4 60 l.speed(-(0.75 * MAXSPEED));
blu12758 2:7a4179249fa4 61 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 62 }
blu12758 1:c319e24af8df 63 else if(this->read() == 0b00000001){
blu12758 2:7a4179249fa4 64 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 65 r.speed(-(0.75 * MAXSPEED));
blu12758 1:c319e24af8df 66 }
blu12758 1:c319e24af8df 67 break;
blu12758 1:c319e24af8df 68
blu12758 1:c319e24af8df 69 case 2: if(this->read() == 0b00011000){
blu12758 2:7a4179249fa4 70 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 71 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 72 }
blu12758 1:c319e24af8df 73 else if(this->read() == 0b11000000){
blu12758 2:7a4179249fa4 74 l.speed(-(0.5 * MAXSPEED));
blu12758 2:7a4179249fa4 75 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 76 }
blu12758 1:c319e24af8df 77 else if(this->read() == 0b00000011){
blu12758 2:7a4179249fa4 78 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 79 r.speed(-(0.5 * MAXSPEED));
blu12758 1:c319e24af8df 80 }
blu12758 1:c319e24af8df 81 else if(this->read() == 0b01100000){
blu12758 2:7a4179249fa4 82 l.speed(0);
blu12758 2:7a4179249fa4 83 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 84 }
blu12758 1:c319e24af8df 85 else if(this->read() == 0b00000110){
blu12758 2:7a4179249fa4 86 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 87 r.speed(0);
blu12758 1:c319e24af8df 88 }
blu12758 1:c319e24af8df 89 else if(this->read() == 0b00110000){
blu12758 2:7a4179249fa4 90 l.speed(0.5*MAXSPEED);
blu12758 2:7a4179249fa4 91 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 92 }
blu12758 1:c319e24af8df 93 else if(this->read() == 0b00001100){
blu12758 2:7a4179249fa4 94 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 95 r.speed(0.5*MAXSPEED);
blu12758 1:c319e24af8df 96 }
blu12758 1:c319e24af8df 97 break;
blu12758 1:c319e24af8df 98
blu12758 1:c319e24af8df 99 case 3: if(this->read() == 0b11100000){
blu12758 2:7a4179249fa4 100 l.speed(-(0.25*MAXSPEED));
blu12758 2:7a4179249fa4 101 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 102 }
blu12758 1:c319e24af8df 103 else if(this->read() == 0b00000111){
blu12758 2:7a4179249fa4 104 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 105 r.speed(-(0.25*MAXSPEED));
blu12758 1:c319e24af8df 106 }
blu12758 1:c319e24af8df 107 else if(this->read() == 0b01110000){
blu12758 2:7a4179249fa4 108 l.speed(0.25*MAXSPEED);
blu12758 2:7a4179249fa4 109 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 110 }
blu12758 1:c319e24af8df 111 else if(this->read() == 0b00001110){
blu12758 2:7a4179249fa4 112 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 113 r.speed(0.25*MAXSPEED);
blu12758 1:c319e24af8df 114 }
blu12758 1:c319e24af8df 115 else if(this->read() == 0b00111000){
blu12758 2:7a4179249fa4 116 l.speed(0.5*MAXSPEED);
blu12758 2:7a4179249fa4 117 r.speed(MAXSPEED);
blu12758 1:c319e24af8df 118 }
blu12758 1:c319e24af8df 119 else if(this->read() == 0b00011100){
blu12758 2:7a4179249fa4 120 l.speed(MAXSPEED);
blu12758 2:7a4179249fa4 121 r.speed(0.5*MAXSPEED);
blu12758 1:c319e24af8df 122 }
blu12758 1:c319e24af8df 123 break
blu12758 1:c319e24af8df 124 default: break;
blu12758 1:c319e24af8df 125 }
blu12758 0:2623af9e8ef3 126
blu12758 0:2623af9e8ef3 127 }