project enzo / Mbed 2 deprecated robot Featured

dit is em

Dependencies:   mbed

main.cpp@19:25663276160d, 2017-05-23 (annotated)

Committer:
bjorntukkertje
Date:
Tue May 23 11:36:21 2017 +0000
Revision:
19:25663276160d
Parent:
18:3a613ea8e28c
Child:
20:9f4ba1b3d06e
niels zijn extratjes;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bjorntukkertje 0:9756a10feb5f 1 #include "mbed.h"
bjorntukkertje 0:9756a10feb5f 2
bjorntukkertje 19:25663276160d 3
joosthartkamp 18:3a613ea8e28c 4 PwmOut motorleftspeed(D6);
joosthartkamp 16:a9382753e8d6 5 PwmOut motorrightspeed(D7);
joosthartkamp 18:3a613ea8e28c 6 DigitalOut MotorL1pin (D12);
joosthartkamp 18:3a613ea8e28c 7 DigitalOut MotorL2pin (D11);
joosthartkamp 18:3a613ea8e28c 8 DigitalOut MotorR1pin (D10);
joosthartkamp 18:3a613ea8e28c 9 DigitalOut MotorR2pin (D9);
bjorntukkertje 6:6659a8ada914 10 AnalogIn Sensor1 (A1);
bjorntukkertje 6:6659a8ada914 11 AnalogIn Sensor2 (A2);
bjorntukkertje 6:6659a8ada914 12 AnalogIn Sensor3 (A3);
bjorntukkertje 6:6659a8ada914 13 AnalogIn Sensor4 (A4);
bjorntukkertje 11:aac90fca0290 14 AnalogIn zoeksensor (A5);
joosthartkamp 16:a9382753e8d6 15 AnalogIn currentleftpin (A6);
joosthartkamp 16:a9382753e8d6 16 AnalogIn currentrightpin (A7);
bjorntukkertje 10:18116d979d61 17 DigitalOut steppera (D2);
bjorntukkertje 10:18116d979d61 18 DigitalOut stepperb (D3);
bjorntukkertje 10:18116d979d61 19 DigitalOut stepperc (D4);
bjorntukkertje 10:18116d979d61 20 DigitalOut stepperd (D5);
joosthartkamp 18:3a613ea8e28c 21 DigitalIn knop(D8);
joosthartkamp 18:3a613ea8e28c 22 DigitalOut led(D1);
bjorntukkertje 0:9756a10feb5f 23
bjorntukkertje 0:9756a10feb5f 24
bjorntukkertje 11:aac90fca0290 25 int afstand = zoeksensor;
bjorntukkertje 11:aac90fca0290 26
bjorntukkertje 10:18116d979d61 27 int stapmode = 0;
bjorntukkertje 10:18116d979d61 28 int position = 0;
bjorntukkertje 10:18116d979d61 29 int pos;
bjorntukkertje 10:18116d979d61 30
bjorntukkertje 19:25663276160d 31
joosthartkamp 18:3a613ea8e28c 32 int mapcurrent(float input = 0.00,float inputmin = 0.00, float inputmax = 0.00,int outputmin = 0, int outputmax = 0)
joosthartkamp 16:a9382753e8d6 33 {
joosthartkamp 16:a9382753e8d6 34 return (input - inputmin) * (outputmax - outputmin) / (inputmax - inputmin) + outputmin;
joosthartkamp 16:a9382753e8d6 35 }
bjorntukkertje 10:18116d979d61 36 // poar neemn
bjorntukkertje 10:18116d979d61 37 // twee poar neemn
bjorntukkertje 10:18116d979d61 38 // twee tettn in n envelop
bjorntukkertje 19:25663276160d 39 //neuken tot de vellen er bij hangen
bjorntukkertje 19:25663276160d 40
joosthartkamp 16:a9382753e8d6 41 int stepper()
bjorntukkertje 10:18116d979d61 42 {
bjorntukkertje 10:18116d979d61 43 switch (stapmode) {
bjorntukkertje 10:18116d979d61 44 case 0:
bjorntukkertje 10:18116d979d61 45 steppera = 1;
bjorntukkertje 10:18116d979d61 46 stepperb = 0;
bjorntukkertje 10:18116d979d61 47 stepperc = 1;
bjorntukkertje 10:18116d979d61 48 stepperd = 0;
bjorntukkertje 10:18116d979d61 49 pos ++;
bjorntukkertje 10:18116d979d61 50 break;
bjorntukkertje 10:18116d979d61 51 case 1:
bjorntukkertje 10:18116d979d61 52 steppera = 1;
bjorntukkertje 10:18116d979d61 53 stepperb = 0;
bjorntukkertje 10:18116d979d61 54 stepperc = 0;
bjorntukkertje 10:18116d979d61 55 stepperd = 1;
bjorntukkertje 10:18116d979d61 56 pos ++;
bjorntukkertje 10:18116d979d61 57 break;
bjorntukkertje 10:18116d979d61 58 case 2:
bjorntukkertje 10:18116d979d61 59 steppera = 0;
bjorntukkertje 10:18116d979d61 60 stepperb = 1;
bjorntukkertje 10:18116d979d61 61 stepperc = 0;
bjorntukkertje 10:18116d979d61 62 stepperd = 1;
bjorntukkertje 10:18116d979d61 63 pos ++;
bjorntukkertje 10:18116d979d61 64 break;
bjorntukkertje 10:18116d979d61 65 case 3:
bjorntukkertje 10:18116d979d61 66 steppera = 0;
bjorntukkertje 10:18116d979d61 67 stepperb = 1;
bjorntukkertje 10:18116d979d61 68 stepperc = 1;
bjorntukkertje 10:18116d979d61 69 stepperd = 0;
bjorntukkertje 10:18116d979d61 70 pos ++;
bjorntukkertje 10:18116d979d61 71 break;
bjorntukkertje 10:18116d979d61 72 ;
bjorntukkertje 10:18116d979d61 73 }
bjorntukkertje 10:18116d979d61 74 if (pos > 1000) { //na volledige rotatie ga naar nul
bjorntukkertje 13:02e56051501a 75 pos = 0;
bjorntukkertje 13:02e56051501a 76 }
joosthartkamp 16:a9382753e8d6 77 stapmode ++;
joosthartkamp 16:a9382753e8d6 78 if (stapmode > 3) {
joosthartkamp 16:a9382753e8d6 79 stapmode = 0;
joosthartkamp 16:a9382753e8d6 80 }
bjorntukkertje 10:18116d979d61 81 return pos;
bjorntukkertje 13:02e56051501a 82
bjorntukkertje 10:18116d979d61 83 }
bjorntukkertje 13:02e56051501a 84
joosthartkamp 18:3a613ea8e28c 85 bool afstandzoeker ()
bjorntukkertje 10:18116d979d61 86 {
joosthartkamp 18:3a613ea8e28c 87 bool a ;
joosthartkamp 18:3a613ea8e28c 88 if (zoeksensor > 0.25) {
joosthartkamp 18:3a613ea8e28c 89 a = 1;
joosthartkamp 18:3a613ea8e28c 90 } else {
joosthartkamp 18:3a613ea8e28c 91 a=0 ;
joosthartkamp 18:3a613ea8e28c 92 }
joosthartkamp 18:3a613ea8e28c 93 return a;
bjorntukkertje 10:18116d979d61 94 }
bjorntukkertje 13:02e56051501a 95
joosthartkamp 18:3a613ea8e28c 96 int lijnsensor ()
joosthartkamp 18:3a613ea8e28c 97
bjorntukkertje 10:18116d979d61 98 {
joosthartkamp 18:3a613ea8e28c 99 bool a = 0;
joosthartkamp 18:3a613ea8e28c 100 bool b = 0;
joosthartkamp 18:3a613ea8e28c 101 bool c = 0;
joosthartkamp 18:3a613ea8e28c 102 bool d = 0;
joosthartkamp 18:3a613ea8e28c 103
joosthartkamp 18:3a613ea8e28c 104 int output;
joosthartkamp 18:3a613ea8e28c 105 if (Sensor1 > 0.01) {
joosthartkamp 18:3a613ea8e28c 106 a = 1;
joosthartkamp 18:3a613ea8e28c 107 } else {
joosthartkamp 18:3a613ea8e28c 108 a = 0;
bjorntukkertje 10:18116d979d61 109 }
joosthartkamp 18:3a613ea8e28c 110 if (Sensor2 > 0.01) {
joosthartkamp 18:3a613ea8e28c 111 b = 1<<1;
joosthartkamp 18:3a613ea8e28c 112 } else {
joosthartkamp 18:3a613ea8e28c 113 b = 0<<1;
joosthartkamp 18:3a613ea8e28c 114 }
joosthartkamp 18:3a613ea8e28c 115 if (Sensor3 > 0.01) {
joosthartkamp 18:3a613ea8e28c 116 c = 1<<2;
joosthartkamp 18:3a613ea8e28c 117 } else {
joosthartkamp 18:3a613ea8e28c 118 c = 0<<2;
joosthartkamp 18:3a613ea8e28c 119 }
joosthartkamp 18:3a613ea8e28c 120 if (Sensor4 > 0.01) {
joosthartkamp 18:3a613ea8e28c 121 d = 1<<3;
joosthartkamp 18:3a613ea8e28c 122 } else {
joosthartkamp 18:3a613ea8e28c 123 d = 0<<3;
joosthartkamp 18:3a613ea8e28c 124 }
joosthartkamp 18:3a613ea8e28c 125
joosthartkamp 18:3a613ea8e28c 126 output = a || b || c || d;
joosthartkamp 18:3a613ea8e28c 127 return output;
bjorntukkertje 10:18116d979d61 128 }
bjorntukkertje 13:02e56051501a 129
bjorntukkertje 8:ec7b7c9ad07e 130
joosthartkamp 18:3a613ea8e28c 131
joosthartkamp 18:3a613ea8e28c 132 void Hbrug(int speedleft = 0, int speedright = 0, int direction = 0)
bjorntukkertje 13:02e56051501a 133 {
joosthartkamp 18:3a613ea8e28c 134 int currenta = mapcurrent(currentleftpin,0,1,0,6600);
joosthartkamp 18:3a613ea8e28c 135 int currentb = mapcurrent(currentrightpin,0,1,0,6600);
bjorntukkertje 13:02e56051501a 136
bjorntukkertje 13:02e56051501a 137
joosthartkamp 18:3a613ea8e28c 138 bool MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 139 bool MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 140 bool MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 141 bool MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 142
joosthartkamp 18:3a613ea8e28c 143 switch (stapmode) {
joosthartkamp 18:3a613ea8e28c 144 case 0:
joosthartkamp 18:3a613ea8e28c 145 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 146 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 147 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 148 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 149 break;
joosthartkamp 18:3a613ea8e28c 150 case 1:
joosthartkamp 18:3a613ea8e28c 151 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 152 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 153 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 154 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 155 break;
joosthartkamp 18:3a613ea8e28c 156 case 2:
joosthartkamp 18:3a613ea8e28c 157 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 158 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 159 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 160 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 161 break;
joosthartkamp 18:3a613ea8e28c 162 case 3:
joosthartkamp 18:3a613ea8e28c 163 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 164 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 165 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 166 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 167 break;
joosthartkamp 18:3a613ea8e28c 168 case 4:
joosthartkamp 18:3a613ea8e28c 169 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 170 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 171 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 172 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 173 break;
joosthartkamp 18:3a613ea8e28c 174 case 5:
joosthartkamp 18:3a613ea8e28c 175 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 176 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 177 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 178 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 179 break;
joosthartkamp 18:3a613ea8e28c 180 case 6:
joosthartkamp 18:3a613ea8e28c 181 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 182 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 183 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 184 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 185 break;
joosthartkamp 18:3a613ea8e28c 186 case 7:
joosthartkamp 18:3a613ea8e28c 187 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 188 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 189 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 190 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 191 break;
joosthartkamp 18:3a613ea8e28c 192 case 8:
joosthartkamp 18:3a613ea8e28c 193 MotorL1 = 0;
joosthartkamp 18:3a613ea8e28c 194 MotorL2 = 0;
joosthartkamp 18:3a613ea8e28c 195 MotorR1 = 0;
joosthartkamp 18:3a613ea8e28c 196 MotorR2 = 0;
joosthartkamp 18:3a613ea8e28c 197 break;
joosthartkamp 18:3a613ea8e28c 198
bjorntukkertje 13:02e56051501a 199 }
joosthartkamp 18:3a613ea8e28c 200 MotorL1pin = MotorL1;
joosthartkamp 18:3a613ea8e28c 201 MotorL2pin = MotorL2;
joosthartkamp 18:3a613ea8e28c 202 MotorR1pin = MotorR1;
joosthartkamp 18:3a613ea8e28c 203 MotorR2pin = MotorR2;
joosthartkamp 18:3a613ea8e28c 204
bjorntukkertje 0:9756a10feb5f 205 }
bjorntukkertje 19:25663276160d 206
bjorntukkertje 19:25663276160d 207 int main (){
bjorntukkertje 19:25663276160d 208
bjorntukkertje 19:25663276160d 209 }