Project
Dependencies: Motor Servo mbed
project.cpp@5:429bbb7077e9, 2018-10-25 (annotated)
- Committer:
- corwinstites
- Date:
- Thu Oct 25 15:03:53 2018 +0000
- Revision:
- 5:429bbb7077e9
- Parent:
- 4:b3bd945920cd
updated
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
corwinstites | 0:17c1875334d9 | 1 | #include "mbed.h" |
corwinstites | 0:17c1875334d9 | 2 | #include "Servo.h" |
corwinstites | 0:17c1875334d9 | 3 | #include "Motor.h" |
corwinstites | 0:17c1875334d9 | 4 | |
corwinstites | 1:ed0f48d7be99 | 5 | void mov1(void); // Defining functions |
corwinstites | 1:ed0f48d7be99 | 6 | void mov2(void); |
corwinstites | 1:ed0f48d7be99 | 7 | void mov3(void); |
corwinstites | 1:ed0f48d7be99 | 8 | |
corwinstites | 1:ed0f48d7be99 | 9 | Servo uparm(p21); // Assigns pins to servo |
corwinstites | 0:17c1875334d9 | 10 | Servo lowarm(p22); |
corwinstites | 1:ed0f48d7be99 | 11 | Motor table(p26, p29, p30); // Assigns DC pins |
corwinstites | 1:ed0f48d7be99 | 12 | |
corwinstites | 1:ed0f48d7be99 | 13 | DigitalIn base(p20); // Assigns switch pins |
corwinstites | 0:17c1875334d9 | 14 | DigitalIn s2(p19); |
corwinstites | 0:17c1875334d9 | 15 | DigitalIn s3(p18); |
corwinstites | 0:17c1875334d9 | 16 | DigitalIn s4(p17); |
corwinstites | 4:b3bd945920cd | 17 | DigitalOut lights[5] = {p5,p6,p7,p8,p11}; // Assigns LED pens |
corwinstites | 1:ed0f48d7be99 | 18 | |
corwinstites | 5:429bbb7077e9 | 19 | int a,b,c,d; |
corwinstites | 0:17c1875334d9 | 20 | |
corwinstites | 0:17c1875334d9 | 21 | int main() |
corwinstites | 0:17c1875334d9 | 22 | { |
corwinstites | 1:ed0f48d7be99 | 23 | |
corwinstites | 1:ed0f48d7be99 | 24 | uparm.calibrate(0.0009,90);// calibrates arm loaations |
corwinstites | 1:ed0f48d7be99 | 25 | lowarm.calibrate(0.0009,90); |
corwinstites | 1:ed0f48d7be99 | 26 | uparm = .5; // halfway points |
corwinstites | 1:ed0f48d7be99 | 27 | lowarm = .5; |
corwinstites | 1:ed0f48d7be99 | 28 | |
corwinstites | 1:ed0f48d7be99 | 29 | |
corwinstites | 1:ed0f48d7be99 | 30 | while(1){ // continuous while statement |
corwinstites | 1:ed0f48d7be99 | 31 | |
corwinstites | 1:ed0f48d7be99 | 32 | a = base.read(); // reads in switch values |
corwinstites | 1:ed0f48d7be99 | 33 | b = s2.read(); |
corwinstites | 1:ed0f48d7be99 | 34 | c = s3.read(); |
corwinstites | 1:ed0f48d7be99 | 35 | d = s4.read(); |
corwinstites | 1:ed0f48d7be99 | 36 | |
corwinstites | 1:ed0f48d7be99 | 37 | if (a==0) { // if switch one is off, entire program does not turn on and arms calibrate |
corwinstites | 1:ed0f48d7be99 | 38 | table.speed(0); |
corwinstites | 1:ed0f48d7be99 | 39 | uparm = .5; |
corwinstites | 1:ed0f48d7be99 | 40 | lowarm = .5; |
corwinstites | 5:429bbb7077e9 | 41 | lights[0]=1; |
corwinstites | 1:ed0f48d7be99 | 42 | lights[1]=0; |
corwinstites | 1:ed0f48d7be99 | 43 | lights[2]=0; |
corwinstites | 1:ed0f48d7be99 | 44 | lights[3]=0; |
corwinstites | 1:ed0f48d7be99 | 45 | lights[4]=0; |
corwinstites | 1:ed0f48d7be99 | 46 | } // end if |
corwinstites | 1:ed0f48d7be99 | 47 | |
corwinstites | 1:ed0f48d7be99 | 48 | else if (a==1 && b==0 && c==0 && d==0) |
corwinstites | 1:ed0f48d7be99 | 49 | { // if only switch one is on, lights are on and table spins. |
corwinstites | 5:429bbb7077e9 | 50 | table.speed(.4); |
corwinstites | 5:429bbb7077e9 | 51 | lights[0]=1; |
corwinstites | 5:429bbb7077e9 | 52 | lights[1]=1; |
corwinstites | 5:429bbb7077e9 | 53 | lights[2]=0; |
corwinstites | 5:429bbb7077e9 | 54 | lights[3]=0; |
corwinstites | 5:429bbb7077e9 | 55 | lights[4]=0; |
corwinstites | 5:429bbb7077e9 | 56 | printf("Turning"); |
corwinstites | 1:ed0f48d7be99 | 57 | } // end if |
corwinstites | 1:ed0f48d7be99 | 58 | |
corwinstites | 1:ed0f48d7be99 | 59 | else if (a==1 && b==1 && c==0 && d==0){ // if switches one and two are on, table spins, and movement 1 happens |
corwinstites | 5:429bbb7077e9 | 60 | table.speed(.4); |
corwinstites | 5:429bbb7077e9 | 61 | lights[0]=1; |
corwinstites | 5:429bbb7077e9 | 62 | lights[1]=1; |
corwinstites | 5:429bbb7077e9 | 63 | lights[2]=1; |
corwinstites | 5:429bbb7077e9 | 64 | lights[3]=0; |
corwinstites | 5:429bbb7077e9 | 65 | lights[4]=0; |
corwinstites | 1:ed0f48d7be99 | 66 | mov1(); |
corwinstites | 5:429bbb7077e9 | 67 | |
corwinstites | 1:ed0f48d7be99 | 68 | } // end if |
corwinstites | 1:ed0f48d7be99 | 69 | else if (a==1 && b==0 && c==1 && d==0){ // if switches one and three on, table spins, movement two happens |
corwinstites | 5:429bbb7077e9 | 70 | table.speed(.4); |
corwinstites | 5:429bbb7077e9 | 71 | lights[0]=1; |
corwinstites | 5:429bbb7077e9 | 72 | lights[1]=1; |
corwinstites | 5:429bbb7077e9 | 73 | lights[2]=1; |
corwinstites | 5:429bbb7077e9 | 74 | lights[3]=1; |
corwinstites | 5:429bbb7077e9 | 75 | lights[4]=0; |
corwinstites | 1:ed0f48d7be99 | 76 | mov2(); |
corwinstites | 5:429bbb7077e9 | 77 | |
corwinstites | 1:ed0f48d7be99 | 78 | } // end if |
corwinstites | 1:ed0f48d7be99 | 79 | else if (a==1 && b==0 && c==0 && d==1){ // if switches one and three are on, table speens, movement three |
corwinstites | 5:429bbb7077e9 | 80 | table.speed(.4); |
corwinstites | 5:429bbb7077e9 | 81 | lights[0]=1; |
corwinstites | 5:429bbb7077e9 | 82 | lights[1]=1; |
corwinstites | 5:429bbb7077e9 | 83 | lights[2]=1; |
corwinstites | 5:429bbb7077e9 | 84 | lights[3]=1; |
corwinstites | 5:429bbb7077e9 | 85 | lights[4]=1; |
corwinstites | 1:ed0f48d7be99 | 86 | mov3(); |
corwinstites | 5:429bbb7077e9 | 87 | |
corwinstites | 1:ed0f48d7be99 | 88 | } // end if |
corwinstites | 1:ed0f48d7be99 | 89 | else{ |
corwinstites | 1:ed0f48d7be99 | 90 | table.speed(0); |
corwinstites | 1:ed0f48d7be99 | 91 | uparm = .5; |
corwinstites | 1:ed0f48d7be99 | 92 | lowarm = .5; |
corwinstites | 5:429bbb7077e9 | 93 | lights[0]=1; |
corwinstites | 1:ed0f48d7be99 | 94 | lights[1]=0; |
corwinstites | 1:ed0f48d7be99 | 95 | lights[2]=0; |
corwinstites | 1:ed0f48d7be99 | 96 | lights[3]=0; |
corwinstites | 1:ed0f48d7be99 | 97 | lights[4]=0; |
corwinstites | 1:ed0f48d7be99 | 98 | } // end if |
corwinstites | 3:99852d2be11d | 99 | wait(.02); |
corwinstites | 1:ed0f48d7be99 | 100 | }// end while |
corwinstites | 1:ed0f48d7be99 | 101 | } // end main |
corwinstites | 1:ed0f48d7be99 | 102 | |
corwinstites | 5:429bbb7077e9 | 103 | |
corwinstites | 1:ed0f48d7be99 | 104 | |
corwinstites | 1:ed0f48d7be99 | 105 | void mov1(void){ |
corwinstites | 1:ed0f48d7be99 | 106 | int i; |
corwinstites | 1:ed0f48d7be99 | 107 | int j; |
corwinstites | 1:ed0f48d7be99 | 108 | lowarm = .5; |
corwinstites | 1:ed0f48d7be99 | 109 | uparm = .5; |
corwinstites | 5:429bbb7077e9 | 110 | while(a==1 && b==1 && c==0 && d==0){ |
corwinstites | 5:429bbb7077e9 | 111 | a = base.read(); // reads in switch values |
corwinstites | 5:429bbb7077e9 | 112 | b = s2.read(); |
corwinstites | 5:429bbb7077e9 | 113 | c = s3.read(); |
corwinstites | 5:429bbb7077e9 | 114 | d = s4.read(); |
corwinstites | 1:ed0f48d7be99 | 115 | for (i=0; i<40; i++){ |
corwinstites | 5:429bbb7077e9 | 116 | uparm = .5 + i*0.0041666; |
corwinstites | 5:429bbb7077e9 | 117 | wait(.038); |
corwinstites | 1:ed0f48d7be99 | 118 | } |
corwinstites | 5:429bbb7077e9 | 119 | uparm = 0.66667; |
corwinstites | 1:ed0f48d7be99 | 120 | for (j=0; j<40; j++){ |
corwinstites | 5:429bbb7077e9 | 121 | uparm = 0.66667-j*0.0041666; |
corwinstites | 5:429bbb7077e9 | 122 | wait(.038); |
corwinstites | 3:99852d2be11d | 123 | printf("Movement 1 \n\r"); |
corwinstites | 1:ed0f48d7be99 | 124 | } |
corwinstites | 1:ed0f48d7be99 | 125 | } |
corwinstites | 5:429bbb7077e9 | 126 | } |
corwinstites | 1:ed0f48d7be99 | 127 | |
corwinstites | 1:ed0f48d7be99 | 128 | |
corwinstites | 1:ed0f48d7be99 | 129 | void mov2(void){ |
corwinstites | 1:ed0f48d7be99 | 130 | int i; |
corwinstites | 1:ed0f48d7be99 | 131 | int j; |
corwinstites | 1:ed0f48d7be99 | 132 | lowarm = .5; |
corwinstites | 1:ed0f48d7be99 | 133 | uparm = .5; |
corwinstites | 5:429bbb7077e9 | 134 | while(a==1 && b==0 && c==1 && d==0){ |
corwinstites | 5:429bbb7077e9 | 135 | a = base.read(); // reads in switch values |
corwinstites | 5:429bbb7077e9 | 136 | b = s2.read(); |
corwinstites | 5:429bbb7077e9 | 137 | c = s3.read(); |
corwinstites | 5:429bbb7077e9 | 138 | d = s4.read(); |
corwinstites | 1:ed0f48d7be99 | 139 | for (i=0; i<40; i++){ |
corwinstites | 5:429bbb7077e9 | 140 | uparm = .5 + i*0.00208333; |
corwinstites | 5:429bbb7077e9 | 141 | lowarm = .5 + i*0.0041666; |
corwinstites | 5:429bbb7077e9 | 142 | wait(.038); |
corwinstites | 1:ed0f48d7be99 | 143 | } |
corwinstites | 5:429bbb7077e9 | 144 | uparm = 0.58333; |
corwinstites | 5:429bbb7077e9 | 145 | lowarm= 0.66667; |
corwinstites | 1:ed0f48d7be99 | 146 | for (j=0; j<40; j++){ |
corwinstites | 5:429bbb7077e9 | 147 | uparm = 0.58333-j*0.00208333; |
corwinstites | 5:429bbb7077e9 | 148 | lowarm = 0.66667-j*0.0041666; |
corwinstites | 5:429bbb7077e9 | 149 | wait(.038); |
corwinstites | 3:99852d2be11d | 150 | printf("Movement 2 \n\r"); |
corwinstites | 1:ed0f48d7be99 | 151 | } |
corwinstites | 1:ed0f48d7be99 | 152 | } |
corwinstites | 5:429bbb7077e9 | 153 | } |
corwinstites | 1:ed0f48d7be99 | 154 | |
corwinstites | 1:ed0f48d7be99 | 155 | void mov3(void) { |
corwinstites | 1:ed0f48d7be99 | 156 | int i; |
corwinstites | 1:ed0f48d7be99 | 157 | int j; |
corwinstites | 1:ed0f48d7be99 | 158 | lowarm = .5; |
corwinstites | 5:429bbb7077e9 | 159 | uparm = 0.66667; |
corwinstites | 5:429bbb7077e9 | 160 | while(a==1 && b==0 && c==0 && d==1){ |
corwinstites | 5:429bbb7077e9 | 161 | a = base.read(); // reads in switch values |
corwinstites | 5:429bbb7077e9 | 162 | b = s2.read(); |
corwinstites | 5:429bbb7077e9 | 163 | c = s3.read(); |
corwinstites | 5:429bbb7077e9 | 164 | d = s4.read(); |
corwinstites | 1:ed0f48d7be99 | 165 | for (i=0; i<40; i++){ |
corwinstites | 5:429bbb7077e9 | 166 | uparm = 0.66667 - i*0.0041666; |
corwinstites | 5:429bbb7077e9 | 167 | lowarm = .5 + i*0.0041666; |
corwinstites | 5:429bbb7077e9 | 168 | wait(.038); |
corwinstites | 1:ed0f48d7be99 | 169 | } |
corwinstites | 1:ed0f48d7be99 | 170 | uparm = 0.5; |
corwinstites | 5:429bbb7077e9 | 171 | lowarm= 0.66667; |
corwinstites | 1:ed0f48d7be99 | 172 | for (j=0; j<40; j++){ |
corwinstites | 5:429bbb7077e9 | 173 | uparm = .5 + j*0.0041666; |
corwinstites | 5:429bbb7077e9 | 174 | lowarm = 0.66667-j*0.0041666; |
corwinstites | 5:429bbb7077e9 | 175 | wait(.038); |
corwinstites | 3:99852d2be11d | 176 | printf("Movement 3 \n\r"); |
corwinstites | 1:ed0f48d7be99 | 177 | } |
corwinstites | 1:ed0f48d7be99 | 178 | } |
corwinstites | 5:429bbb7077e9 | 179 | } |