ASEE-2014 / LineFollower

First team commit

Dependents:   ASEE-2014

Fork of LineFollower by Christopher Bradford

LineFollower.cpp@11:88aa683f1c94, 2014-03-24 (annotated)

Committer:
blu12758
Date:
Mon Mar 24 00:49:23 2014 +0000
Revision:
11:88aa683f1c94
Parent:
10:a9233fa6240d 
yeah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
blu12758 0:2623af9e8ef3 1 #include "LineFollower.h"
blu12758 4:2abe380c57f1 2 #include "Motor.h"
blu12758 0:2623af9e8ef3 3 #include "mbed.h"
blu12758 0:2623af9e8ef3 4 #include <stdint.h>
blu12758 0:2623af9e8ef3 5
blu12758 0:2623af9e8ef3 6 /** Create a Line Follower interface for an IR Sensor Array
blu12758 0:2623af9e8ef3 7 *
blu12758 0:2623af9e8ef3 8 * @param ir1 IR Sensor 1
blu12758 0:2623af9e8ef3 9 * @param ir2 IR Sensor 2
blu12758 0:2623af9e8ef3 10 * @param ir3 IR Sensor 3
blu12758 0:2623af9e8ef3 11 * @param ir4 IR Sensor 4
blu12758 0:2623af9e8ef3 12 * @param ir5 IR Sensor 5
blu12758 0:2623af9e8ef3 13 * @param ir6 IR Sensor 6
blu12758 0:2623af9e8ef3 14 * @param ir7 IR Sensor 7
blu12758 0:2623af9e8ef3 15 * @param ir8 IR Sensor 8
blu12758 0:2623af9e8ef3 16 */
blu12758 4:2abe380c57f1 17 LineFollower::LineFollower(PinName ir1, PinName ir2, PinName ir3, PinName ir4,
blu12758 4:2abe380c57f1 18 PinName ir5, PinName ir6, PinName ir7, PinName ir8):
blu12758 4:2abe380c57f1 19 array(ir1, ir2, ir3, ir4, ir5, ir6, ir7, ir8)
blu12758 4:2abe380c57f1 20 {}
blu12758 0:2623af9e8ef3 21
blu12758 0:2623af9e8ef3 22 /** Read the value of a LineFollower object
blu12758 0:2623af9e8ef3 23 *
blu12758 0:2623af9e8ef3 24 * @return The value of the Sensor
blu12758 0:2623af9e8ef3 25 */
blu12758 0:2623af9e8ef3 26 uint8_t LineFollower::read(){
blu12758 4:2abe380c57f1 27 return array;
blu12758 1:c319e24af8df 28 }
blu12758 1:c319e24af8df 29
blu12758 10:a9233fa6240d 30 float LineFollower::analogFollow(Motor l, Motor r, float maxspeed, float sp_adj_mult){
dfvreeland 9:039a74519b89 31 float sum = 0;
dfvreeland 9:039a74519b89 32 int count = 0;
dfvreeland 9:039a74519b89 33 for(int i = 0; i < 8; i++)
dfvreeland 9:039a74519b89 34 if(!(array & (1<<i))){
dfvreeland 9:039a74519b89 35 sum += (i+1);
dfvreeland 9:039a74519b89 36 count++;
dfvreeland 9:039a74519b89 37 }
blu12758 11:88aa683f1c94 38 if(count == 0 || count == 8) return -1;
dfvreeland 9:039a74519b89 39 float center = sum/count;
blu12758 10:a9233fa6240d 40 //Change the sp_adj_mult to change how drastic your adjustments are. 0-1
blu12758 10:a9233fa6240d 41 float speed_adjust = sp_adj_mult*(center-4.5)/(3.5)*maxspeed;
blu12758 10:a9233fa6240d 42 //Also, can change maxspeed if the thing is too fast overall. 0-1
dfvreeland 9:039a74519b89 43
dfvreeland 9:039a74519b89 44 if(center > 4.5){
blu12758 10:a9233fa6240d 45 l.speed(maxspeed - speed_adjust);
blu12758 10:a9233fa6240d 46 r.speed(maxspeed);
dfvreeland 9:039a74519b89 47 }
dfvreeland 9:039a74519b89 48 else if(center < 4.5){
blu12758 10:a9233fa6240d 49 l.speed(maxspeed);
blu12758 10:a9233fa6240d 50 r.speed(maxspeed + speed_adjust);
dfvreeland 9:039a74519b89 51 }
dfvreeland 9:039a74519b89 52 else{
blu12758 10:a9233fa6240d 53 l.speed(maxspeed);
blu12758 10:a9233fa6240d 54 r.speed(maxspeed);
dfvreeland 9:039a74519b89 55 }
dfvreeland 9:039a74519b89 56
dfvreeland 9:039a74519b89 57 return center;
dfvreeland 9:039a74519b89 58 }
dfvreeland 9:039a74519b89 59
blu12758 1:c319e24af8df 60 /** Follow a line
blu12758 1:c319e24af8df 61 *
blu12758 1:c319e24af8df 62 * @param l left drive motor
blu12758 1:c319e24af8df 63 * @param r right drive motor
blu12758 10:a9233fa6240d 64 */
blu12758 10:a9233fa6240d 65 /*
cbradford 6:3482f3f93988 66 int LineFollower::followLine(Motor l, Motor r){
blu12758 1:c319e24af8df 67 int count = 0;
blu12758 1:c319e24af8df 68 for(int i = 0; i<8; i++){
blu12758 8:12751953ad8e 69 count += (array&(1<<i))?0:1;
blu12758 1:c319e24af8df 70 }
blu12758 1:c319e24af8df 71
blu12758 1:c319e24af8df 72 switch(count){
blu12758 1:c319e24af8df 73
blu12758 8:12751953ad8e 74 case 1: if(this->read() == 0x7F){
blu12758 10:a9233fa6240d 75 l.speed(-(0.75 * maxspeed));
blu12758 10:a9233fa6240d 76 r.speed(maxspeed);
blu12758 1:c319e24af8df 77 }
cbradford 5:1c4c12596ad2 78 else if(this->read() == 0xFE){
blu12758 10:a9233fa6240d 79 l.speed(maxspeed);
blu12758 10:a9233fa6240d 80 r.speed(-(0.75 * maxspeed));
blu12758 1:c319e24af8df 81 }
blu12758 1:c319e24af8df 82 break;
blu12758 1:c319e24af8df 83
blu12758 8:12751953ad8e 84 case 2: if(this->read() == 0xE7){
blu12758 10:a9233fa6240d 85 l.speed(maxspeed);
blu12758 10:a9233fa6240d 86 r.speed(maxspeed);
blu12758 1:c319e24af8df 87 }
cbradford 5:1c4c12596ad2 88 else if(this->read() == 0x3F){
blu12758 10:a9233fa6240d 89 l.speed(-(0.5 * maxspeed));
blu12758 10:a9233fa6240d 90 r.speed(maxspeed);
blu12758 1:c319e24af8df 91 }
cbradford 5:1c4c12596ad2 92 else if(this->read() == 0xFC){
blu12758 10:a9233fa6240d 93 l.speed(maxspeed);
blu12758 10:a9233fa6240d 94 r.speed(-(0.5 * maxspeed));
blu12758 1:c319e24af8df 95 }
cbradford 5:1c4c12596ad2 96 else if(this->read() == 0x9F){
blu12758 2:7a4179249fa4 97 l.speed(0);
blu12758 10:a9233fa6240d 98 r.speed(maxspeed);
blu12758 1:c319e24af8df 99 }
cbradford 5:1c4c12596ad2 100 else if(this->read() == 0xF9){
blu12758 10:a9233fa6240d 101 l.speed(maxspeed);
blu12758 2:7a4179249fa4 102 r.speed(0);
blu12758 1:c319e24af8df 103 }
cbradford 5:1c4c12596ad2 104 else if(this->read() == 0xCF){
blu12758 10:a9233fa6240d 105 l.speed(0.5*maxspeed);
blu12758 10:a9233fa6240d 106 r.speed(maxspeed);
blu12758 1:c319e24af8df 107 }
cbradford 5:1c4c12596ad2 108 else if(this->read() == 0xF3){
blu12758 10:a9233fa6240d 109 l.speed(maxspeed);
blu12758 10:a9233fa6240d 110 r.speed(0.5*maxspeed);
blu12758 1:c319e24af8df 111 }
blu12758 1:c319e24af8df 112 break;
blu12758 1:c319e24af8df 113
blu12758 8:12751953ad8e 114 case 3: if(this->read() == 0x1F){
blu12758 10:a9233fa6240d 115 l.speed(-(0.25*maxspeed));
blu12758 10:a9233fa6240d 116 r.speed(maxspeed);
blu12758 1:c319e24af8df 117 }
cbradford 5:1c4c12596ad2 118 else if(this->read() == 0xF8){
blu12758 10:a9233fa6240d 119 l.speed(maxspeed);
blu12758 10:a9233fa6240d 120 r.speed(-(0.25*maxspeed));
blu12758 1:c319e24af8df 121 }
cbradford 5:1c4c12596ad2 122 else if(this->read() == 0x8F){
blu12758 10:a9233fa6240d 123 l.speed(0.25*maxspeed);
blu12758 10:a9233fa6240d 124 r.speed(maxspeed);
blu12758 1:c319e24af8df 125 }
cbradford 5:1c4c12596ad2 126 else if(this->read() == 0xF1){
blu12758 10:a9233fa6240d 127 l.speed(maxspeed);
blu12758 10:a9233fa6240d 128 r.speed(0.25*maxspeed);
blu12758 1:c319e24af8df 129 }
cbradford 5:1c4c12596ad2 130 else if(this->read() == 0xC7){
blu12758 10:a9233fa6240d 131 l.speed(0.5*maxspeed);
blu12758 10:a9233fa6240d 132 r.speed(maxspeed);
blu12758 1:c319e24af8df 133 }
cbradford 5:1c4c12596ad2 134 else if(this->read() == 0xE3){
blu12758 10:a9233fa6240d 135 l.speed(maxspeed);
blu12758 10:a9233fa6240d 136 r.speed(0.5*maxspeed);
blu12758 1:c319e24af8df 137 }
blu12758 4:2abe380c57f1 138 break;
blu12758 1:c319e24af8df 139 default: break;
cbradford 6:3482f3f93988 140
blu12758 1:c319e24af8df 141 }
dfvreeland 9:039a74519b89 142
dfvreeland 9:039a74519b89 143
dfvreeland 9:039a74519b89 144
cbradford 6:3482f3f93988 145 return count;
blu12758 10:a9233fa6240d 146 }
blu12758 10:a9233fa6240d 147 */