TDP3 Go-Yeti / Mbed 2 deprecated DontKillMbed

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@1:94577052d858, 2016-02-25 (annotated)

Committer:
ibrahim1995
Date:
Thu Feb 25 10:27:59 2016 +0000
Revision:
1:94577052d858
Parent:
0:e679a1bb6eca
Child:
2:8435af9e917a
Modified on 24/02/2016

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joankangro 0:e679a1bb6eca 1 #include "mbed.h"
joankangro 0:e679a1bb6eca 2
joankangro 0:e679a1bb6eca 3 //Pull everything up
ibrahim1995 1:94577052d858 4 DigitalOut pull1(PTA1);
ibrahim1995 1:94577052d858 5 DigitalOut pull2(PTA2);
ibrahim1995 1:94577052d858 6 //DigitalOut pull4(PTA12);
ibrahim1995 1:94577052d858 7 //DigitalOut pull5(PTA4);
ibrahim1995 1:94577052d858 8 DigitalOut pull6(PTA5);
ibrahim1995 1:94577052d858 9 DigitalOut pull7(PTC8);
joankangro 0:e679a1bb6eca 10
ibrahim1995 1:94577052d858 11 DigitalOut pull9(PTA13);
ibrahim1995 1:94577052d858 12 DigitalOut pull10(PTD5);
ibrahim1995 1:94577052d858 13 DigitalOut pull11(PTD0);
ibrahim1995 1:94577052d858 14 DigitalOut pull12(PTD2);
ibrahim1995 1:94577052d858 15 DigitalOut pull13(PTD3);
ibrahim1995 1:94577052d858 16 DigitalOut pull14(PTD1);
ibrahim1995 1:94577052d858 17 DigitalOut pull15(PTE0);
ibrahim1995 1:94577052d858 18 DigitalOut pull16(PTE1);
joankangro 0:e679a1bb6eca 19
ibrahim1995 1:94577052d858 20 //DigitalOut pull17(PTC7);
ibrahim1995 1:94577052d858 21 //DigitalOut pull18(PTC0);
ibrahim1995 1:94577052d858 22 DigitalOut pull19(PTC3);
ibrahim1995 1:94577052d858 23 DigitalOut pull20(PTC4);
joankangro 0:e679a1bb6eca 24
ibrahim1995 1:94577052d858 25 DigitalOut pull21(PTC12);
ibrahim1995 1:94577052d858 26 DigitalOut pull22(PTC13);
ibrahim1995 1:94577052d858 27 DigitalOut pull23(PTC16);
ibrahim1995 1:94577052d858 28 DigitalOut pull24(PTC17);
ibrahim1995 1:94577052d858 29 DigitalOut pull25(PTA16);
ibrahim1995 1:94577052d858 30 DigitalOut pull26(PTA17);
ibrahim1995 1:94577052d858 31 DigitalOut pull27(PTE31);
ibrahim1995 1:94577052d858 32 DigitalOut pull28(PTD6);
ibrahim1995 1:94577052d858 33 DigitalOut pull29(PTD7);
ibrahim1995 1:94577052d858 34
ibrahim1995 1:94577052d858 35 DigitalOut pull30(PTE30);
joankangro 0:e679a1bb6eca 36
ibrahim1995 1:94577052d858 37 //DigitalOut pull38(PTE3);
ibrahim1995 1:94577052d858 38 DigitalOut pull39(PTE2);
ibrahim1995 1:94577052d858 39 //DigitalOut pull50(PTB11);
ibrahim1995 1:94577052d858 40 //DigitalOut pull51(PTB10);
ibrahim1995 1:94577052d858 41 //DigitalOut pull52(PTB9);
ibrahim1995 1:94577052d858 42 //DigitalOut pull53(PTB8);
joankangro 0:e679a1bb6eca 43
ibrahim1995 1:94577052d858 44 //DigitalOut pull34(PTE21);
ibrahim1995 1:94577052d858 45 //DigitalOut pull35(PTE20);
ibrahim1995 1:94577052d858 46 //DigitalOut pull47(PTB1);
ibrahim1995 1:94577052d858 47 //DigitalOut pull48(PTB0);
joankangro 0:e679a1bb6eca 48
joankangro 0:e679a1bb6eca 49
joankangro 0:e679a1bb6eca 50 //motor driver outputs
joankangro 0:e679a1bb6eca 51 DigitalOut IN4(PTC5);
joankangro 0:e679a1bb6eca 52 DigitalOut IN3(PTC6);
joankangro 0:e679a1bb6eca 53 DigitalOut IN2(PTC10);
joankangro 0:e679a1bb6eca 54 DigitalOut IN1(PTC11);
joankangro 0:e679a1bb6eca 55 PwmOut ENA(PTC9);
joankangro 0:e679a1bb6eca 56 PwmOut ENB(PTD4);
joankangro 0:e679a1bb6eca 57
joankangro 0:e679a1bb6eca 58 DigitalOut indicator(LED_BLUE);
joankangro 0:e679a1bb6eca 59
joankangro 0:e679a1bb6eca 60 //constant 3.3 volts for sensors
joankangro 0:e679a1bb6eca 61 DigitalOut constant1(PTE5);
joankangro 0:e679a1bb6eca 62 DigitalOut constant2(PTE4);
ibrahim1995 1:94577052d858 63 DigitalOut constant3(PTE3);
ibrahim1995 1:94577052d858 64 DigitalOut constant4(PTC0);
ibrahim1995 1:94577052d858 65 DigitalOut constant5(PTC7);
joankangro 0:e679a1bb6eca 66
joankangro 0:e679a1bb6eca 67 //line following inputs
ibrahim1995 1:94577052d858 68 AnalogIn leftmost(PTB0);
joankangro 0:e679a1bb6eca 69 AnalogIn left(PTC2);
ibrahim1995 1:94577052d858 70 AnalogIn middle(PTB3);
ibrahim1995 1:94577052d858 71 AnalogIn right(PTB2);
ibrahim1995 1:94577052d858 72 AnalogIn rightmost(PTB1);
ibrahim1995 1:94577052d858 73
ibrahim1995 1:94577052d858 74 DigitalIn back(PTB11);
ibrahim1995 1:94577052d858 75 DigitalIn front(PTB10);
joankangro 0:e679a1bb6eca 76
joankangro 0:e679a1bb6eca 77 //debugging led outputs
ibrahim1995 1:94577052d858 78 DigitalOut ledleftmost(PTE20);
ibrahim1995 1:94577052d858 79 DigitalOut ledright(PTE23);
ibrahim1995 1:94577052d858 80 DigitalOut ledmiddle(PTE22);
ibrahim1995 1:94577052d858 81 DigitalOut ledleft(PTE29);
ibrahim1995 1:94577052d858 82 DigitalOut ledrightmost(PTE21);
ibrahim1995 1:94577052d858 83
ibrahim1995 1:94577052d858 84 DigitalOut ledback(PTB8);
ibrahim1995 1:94577052d858 85 DigitalOut ledfront(PTB9);
ibrahim1995 1:94577052d858 86
ibrahim1995 1:94577052d858 87 //ball firing
ibrahim1995 1:94577052d858 88 PwmOut motors(PTA12);
ibrahim1995 1:94577052d858 89 DigitalOut solenoid(PTA4);
joankangro 0:e679a1bb6eca 90
joankangro 0:e679a1bb6eca 91 int main() {
joankangro 0:e679a1bb6eca 92
ibrahim1995 1:94577052d858 93
ibrahim1995 1:94577052d858 94
ibrahim1995 1:94577052d858 95 pull1=0;
ibrahim1995 1:94577052d858 96 pull2=0;
joankangro 0:e679a1bb6eca 97 //pull3.mode(PullUp);
ibrahim1995 1:94577052d858 98 //pull4=0;
ibrahim1995 1:94577052d858 99 //pull5=0;
ibrahim1995 1:94577052d858 100 pull6=0;
ibrahim1995 1:94577052d858 101 pull7=0;
joankangro 0:e679a1bb6eca 102 //pull8.mode(PullUp);
ibrahim1995 1:94577052d858 103 pull9=0;
ibrahim1995 1:94577052d858 104 pull10=0;
ibrahim1995 1:94577052d858 105 pull11=0;
ibrahim1995 1:94577052d858 106 pull12=0;
ibrahim1995 1:94577052d858 107 pull13=0;
ibrahim1995 1:94577052d858 108 pull14=0;
ibrahim1995 1:94577052d858 109 pull15=0;
ibrahim1995 1:94577052d858 110 pull16=0;
ibrahim1995 1:94577052d858 111 //pull17=0;
ibrahim1995 1:94577052d858 112 //pull18=0;
ibrahim1995 1:94577052d858 113 pull19=0;
ibrahim1995 1:94577052d858 114 pull20=0;
ibrahim1995 1:94577052d858 115 pull21=0;
ibrahim1995 1:94577052d858 116 pull22=0;
ibrahim1995 1:94577052d858 117 pull23=0;
ibrahim1995 1:94577052d858 118 pull24=0;
ibrahim1995 1:94577052d858 119 pull25=0;
ibrahim1995 1:94577052d858 120 pull26=0;
ibrahim1995 1:94577052d858 121 pull27=0;
ibrahim1995 1:94577052d858 122 pull28=0;
ibrahim1995 1:94577052d858 123 pull29=0;
ibrahim1995 1:94577052d858 124 pull30=0;
ibrahim1995 1:94577052d858 125 //pull34=0;
ibrahim1995 1:94577052d858 126 //pull35=0;
ibrahim1995 1:94577052d858 127 //pull38=0;
ibrahim1995 1:94577052d858 128 pull39=0;
ibrahim1995 1:94577052d858 129 //pull47=0;
ibrahim1995 1:94577052d858 130 //pull48=0;
ibrahim1995 1:94577052d858 131 //pull49=0;
ibrahim1995 1:94577052d858 132 //pull50=0;
ibrahim1995 1:94577052d858 133 //pull51=0;
ibrahim1995 1:94577052d858 134 //pull52=0;
ibrahim1995 1:94577052d858 135 //pull53=0;
ibrahim1995 1:94577052d858 136
ibrahim1995 1:94577052d858 137
ibrahim1995 1:94577052d858 138
ibrahim1995 1:94577052d858 139
ibrahim1995 1:94577052d858 140 motors.period(3.0);
ibrahim1995 1:94577052d858 141 motors.write(0);
ibrahim1995 1:94577052d858 142 solenoid=0;
ibrahim1995 1:94577052d858 143
ibrahim1995 1:94577052d858 144 constant1=1;
ibrahim1995 1:94577052d858 145 constant2=1;
ibrahim1995 1:94577052d858 146 constant3=1;
ibrahim1995 1:94577052d858 147 constant4=1;
ibrahim1995 1:94577052d858 148 constant5=1;
joankangro 0:e679a1bb6eca 149
joankangro 0:e679a1bb6eca 150 indicator=1;
joankangro 0:e679a1bb6eca 151 wait(0.3);
joankangro 0:e679a1bb6eca 152 indicator=0;
joankangro 0:e679a1bb6eca 153 wait(0.3);
joankangro 0:e679a1bb6eca 154 indicator=1;
joankangro 0:e679a1bb6eca 155 wait(0.3);
joankangro 0:e679a1bb6eca 156 indicator=0;
joankangro 0:e679a1bb6eca 157 wait(0.3);
joankangro 0:e679a1bb6eca 158 indicator=1;
joankangro 0:e679a1bb6eca 159
joankangro 0:e679a1bb6eca 160 ENA.period(0.001);
joankangro 0:e679a1bb6eca 161 ENB.period(0.001);
joankangro 0:e679a1bb6eca 162
joankangro 0:e679a1bb6eca 163 IN1=0;
joankangro 0:e679a1bb6eca 164 IN2=1;
joankangro 0:e679a1bb6eca 165 IN3=0;
joankangro 0:e679a1bb6eca 166 IN4=1;
joankangro 0:e679a1bb6eca 167 //set inital pwm values for motor driver outputs
joankangro 0:e679a1bb6eca 168 ENA=0;
joankangro 0:e679a1bb6eca 169 ENB=0;
joankangro 0:e679a1bb6eca 170
ibrahim1995 1:94577052d858 171 int counter=0;
ibrahim1995 1:94577052d858 172
joankangro 0:e679a1bb6eca 173 while(1) {
joankangro 0:e679a1bb6eca 174
ibrahim1995 1:94577052d858 175 counter=counter+1;
joankangro 0:e679a1bb6eca 176
ibrahim1995 1:94577052d858 177 if(middle<0.4 & left<0.4 & right<0.4 & leftmost<0.4 & rightmost<0.24){
ibrahim1995 1:94577052d858 178 //skip the loop
ibrahim1995 1:94577052d858 179 ledmiddle=0;
ibrahim1995 1:94577052d858 180 ledleft=0;
ibrahim1995 1:94577052d858 181 ledright=0;
ibrahim1995 1:94577052d858 182 ledleftmost=0;
ibrahim1995 1:94577052d858 183 ledrightmost=0;
joankangro 0:e679a1bb6eca 184 }else{
ibrahim1995 1:94577052d858 185
ibrahim1995 1:94577052d858 186 if(front==1){
ibrahim1995 1:94577052d858 187 ledfront=1;
ibrahim1995 1:94577052d858 188 }else{
ibrahim1995 1:94577052d858 189 ledfront=0;
ibrahim1995 1:94577052d858 190 }
ibrahim1995 1:94577052d858 191
ibrahim1995 1:94577052d858 192 if(back==1){
ibrahim1995 1:94577052d858 193 ledback=1;
ibrahim1995 1:94577052d858 194 }else{
ibrahim1995 1:94577052d858 195 ledback=0;
ibrahim1995 1:94577052d858 196 }
ibrahim1995 1:94577052d858 197
ibrahim1995 1:94577052d858 198 if(back==1 and counter>70){
ibrahim1995 1:94577052d858 199 counter=0;
ibrahim1995 1:94577052d858 200 ENB=0;
ibrahim1995 1:94577052d858 201 ENA=0;
ibrahim1995 1:94577052d858 202 if(front==0){
ibrahim1995 1:94577052d858 203 //shoot balls
ibrahim1995 1:94577052d858 204 motors=0.08;
ibrahim1995 1:94577052d858 205 wait_ms(550);
ibrahim1995 1:94577052d858 206 solenoid=1;
ibrahim1995 1:94577052d858 207 wait_ms(45);
ibrahim1995 1:94577052d858 208 solenoid=0;
ibrahim1995 1:94577052d858 209 }
ibrahim1995 1:94577052d858 210 wait(3);
ibrahim1995 1:94577052d858 211 motors=0;
ibrahim1995 1:94577052d858 212 }
ibrahim1995 1:94577052d858 213
ibrahim1995 1:94577052d858 214
ibrahim1995 1:94577052d858 215
ibrahim1995 1:94577052d858 216 if(middle>0.4){
ibrahim1995 1:94577052d858 217 ledmiddle=1;
ibrahim1995 1:94577052d858 218 ENA=0.5;
ibrahim1995 1:94577052d858 219 ENB=0.5;
ibrahim1995 1:94577052d858 220 }else{
ibrahim1995 1:94577052d858 221 ledmiddle=0;
ibrahim1995 1:94577052d858 222 ENA=0.4;
ibrahim1995 1:94577052d858 223 ENB=0.4;
ibrahim1995 1:94577052d858 224 }
ibrahim1995 1:94577052d858 225
ibrahim1995 1:94577052d858 226 if(left>0.4){
ibrahim1995 1:94577052d858 227 ledleft=1;
ibrahim1995 1:94577052d858 228 indicator=1;
ibrahim1995 1:94577052d858 229 ENA=0.28;
ibrahim1995 1:94577052d858 230 }else{
ibrahim1995 1:94577052d858 231 ledleft=0;
ibrahim1995 1:94577052d858 232 }
joankangro 0:e679a1bb6eca 233
ibrahim1995 1:94577052d858 234 if(right>0.4){
ibrahim1995 1:94577052d858 235 ledright=1;
ibrahim1995 1:94577052d858 236 ENB=0.28;
ibrahim1995 1:94577052d858 237 }else{
ibrahim1995 1:94577052d858 238 ledright=0;
ibrahim1995 1:94577052d858 239 }
ibrahim1995 1:94577052d858 240
ibrahim1995 1:94577052d858 241 if(leftmost>0.4){
ibrahim1995 1:94577052d858 242 ledleftmost=1;
ibrahim1995 1:94577052d858 243 ENA=0;
ibrahim1995 1:94577052d858 244 }else{
ibrahim1995 1:94577052d858 245 ledleftmost=0;
ibrahim1995 1:94577052d858 246 }
joankangro 0:e679a1bb6eca 247
ibrahim1995 1:94577052d858 248 if(rightmost>0.24){
ibrahim1995 1:94577052d858 249 ledrightmost=1;
ibrahim1995 1:94577052d858 250 ENB=0;
ibrahim1995 1:94577052d858 251 }else{
ibrahim1995 1:94577052d858 252 ledrightmost=0;
ibrahim1995 1:94577052d858 253 }
ibrahim1995 1:94577052d858 254
ibrahim1995 1:94577052d858 255
ibrahim1995 1:94577052d858 256 if((left>0.4 or leftmost>0.4)and(right>0.4 or rightmost>0.24)){
ibrahim1995 1:94577052d858 257 ENB=0.4;
ibrahim1995 1:94577052d858 258 ENA=0;
ibrahim1995 1:94577052d858 259 wait(0.5);
ibrahim1995 1:94577052d858 260 }
ibrahim1995 1:94577052d858 261
joankangro 0:e679a1bb6eca 262 }
ibrahim1995 1:94577052d858 263
ibrahim1995 1:94577052d858 264 wait(0.1);
joankangro 0:e679a1bb6eca 265
joankangro 0:e679a1bb6eca 266
joankangro 0:e679a1bb6eca 267 }
joankangro 0:e679a1bb6eca 268 }