Shahan Najam
Final project 4180 (incomplete)
movement.cpp@0:6893dd9a88b3, 2011-12-14 (annotated)
- Committer:
- Shahan
- Date:
- Wed Dec 14 04:58:43 2011 +0000
- Revision:
- 0:6893dd9a88b3
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| Shahan | 0:6893dd9a88b3 | 1 | #include "mbed.h" |
| Shahan | 0:6893dd9a88b3 | 2 | #include "Motor.h" |
| Shahan | 0:6893dd9a88b3 | 3 | #include "movement.h" |
| Shahan | 0:6893dd9a88b3 | 4 | |
| Shahan | 0:6893dd9a88b3 | 5 | |
| Shahan | 0:6893dd9a88b3 | 6 | void moveforward(Motor wheel,float speed){ |
| Shahan | 0:6893dd9a88b3 | 7 | wheel.speed(speed); |
| Shahan | 0:6893dd9a88b3 | 8 | } |
| Shahan | 0:6893dd9a88b3 | 9 | |
| Shahan | 0:6893dd9a88b3 | 10 | void movebackward(Motor wheel,float speed){ |
| Shahan | 0:6893dd9a88b3 | 11 | wheel.speed(-speed); |
| Shahan | 0:6893dd9a88b3 | 12 | } |
| Shahan | 0:6893dd9a88b3 | 13 | |
| Shahan | 0:6893dd9a88b3 | 14 | void move(int distance){ |
| Shahan | 0:6893dd9a88b3 | 15 | rightspokecountrequired = abs(distance)/spokeconversionfactor; |
| Shahan | 0:6893dd9a88b3 | 16 | leftspokecountrequired = abs(distance)/spokeconversionfactor; |
| Shahan | 0:6893dd9a88b3 | 17 | if(distance > 0){ |
| Shahan | 0:6893dd9a88b3 | 18 | moveforward(motor_right,rightmotorspeed); |
| Shahan | 0:6893dd9a88b3 | 19 | moveforward(motor_left,leftmotorspeed); |
| Shahan | 0:6893dd9a88b3 | 20 | } |
| Shahan | 0:6893dd9a88b3 | 21 | else if(distance < 0){ |
| Shahan | 0:6893dd9a88b3 | 22 | movebackward(motor_right,rightmotorspeed); |
| Shahan | 0:6893dd9a88b3 | 23 | movebackward(motor_left,leftmotorspeed); |
| Shahan | 0:6893dd9a88b3 | 24 | } |
| Shahan | 0:6893dd9a88b3 | 25 | } |
| Shahan | 0:6893dd9a88b3 | 26 | |
| Shahan | 0:6893dd9a88b3 | 27 | |
| Shahan | 0:6893dd9a88b3 | 28 | void QTIsensor_init(){ |
| Shahan | 0:6893dd9a88b3 | 29 | rightsensorcontrol = 0; |
| Shahan | 0:6893dd9a88b3 | 30 | leftsensorcontrol = 0; |
| Shahan | 0:6893dd9a88b3 | 31 | } |
| Shahan | 0:6893dd9a88b3 | 32 | |
| Shahan | 0:6893dd9a88b3 | 33 | void QTIsensor_charge(){ |
| Shahan | 0:6893dd9a88b3 | 34 | rightsensorcontrol = 1; |
| Shahan | 0:6893dd9a88b3 | 35 | leftsensorcontrol = 1; |
| Shahan | 0:6893dd9a88b3 | 36 | } |
| Shahan | 0:6893dd9a88b3 | 37 | |
| Shahan | 0:6893dd9a88b3 | 38 | void QTIsensor_discharge(){ |
| Shahan | 0:6893dd9a88b3 | 39 | if(rightsensorval < qtithreshold) rightspokecount++; |
| Shahan | 0:6893dd9a88b3 | 40 | rightsensorcontrol = 0; |
| Shahan | 0:6893dd9a88b3 | 41 | if(leftsensorval < qtithreshold) leftspokecount++; |
| Shahan | 0:6893dd9a88b3 | 42 | leftsensorcontrol = 0; |
| Shahan | 0:6893dd9a88b3 | 43 | } |
| Shahan | 0:6893dd9a88b3 | 44 | |
| Shahan | 0:6893dd9a88b3 | 45 | void motors_stop(){ |
| Shahan | 0:6893dd9a88b3 | 46 | motor_right.speed(0); |
| Shahan | 0:6893dd9a88b3 | 47 | motor_left.speed(0); |
| Shahan | 0:6893dd9a88b3 | 48 | } |
| Shahan | 0:6893dd9a88b3 | 49 | |
| Shahan | 0:6893dd9a88b3 | 50 | void calibratemotors(){ |
| Shahan | 0:6893dd9a88b3 | 51 | int r,l; |
| Shahan | 0:6893dd9a88b3 | 52 | pc.printf("now moving forwards\n\r"); |
| Shahan | 0:6893dd9a88b3 | 53 | move(2); |
| Shahan | 0:6893dd9a88b3 | 54 | while ((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){ |
| Shahan | 0:6893dd9a88b3 | 55 | pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount); |
| Shahan | 0:6893dd9a88b3 | 56 | wait(0.4); |
| Shahan | 0:6893dd9a88b3 | 57 | } |
| Shahan | 0:6893dd9a88b3 | 58 | motors_stop(); |
| Shahan | 0:6893dd9a88b3 | 59 | pc.printf("now moving backwards\n\r"); |
| Shahan | 0:6893dd9a88b3 | 60 | wait(0.3); |
| Shahan | 0:6893dd9a88b3 | 61 | //rightspokecountrequired = 2/spokeconversionfactor; |
| Shahan | 0:6893dd9a88b3 | 62 | //leftspokecountrequired = 2/spokeconversionfactor; |
| Shahan | 0:6893dd9a88b3 | 63 | move(-2); |
| Shahan | 0:6893dd9a88b3 | 64 | pc.printf("right required = %d, left required = %d",rightspokecountrequired,leftspokecountrequired); |
| Shahan | 0:6893dd9a88b3 | 65 | while ((rightspokecount < rightspokecountrequired) && (leftspokecount < leftspokecountrequired)){ |
| Shahan | 0:6893dd9a88b3 | 66 | pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount); |
| Shahan | 0:6893dd9a88b3 | 67 | wait(0.4); |
| Shahan | 0:6893dd9a88b3 | 68 | } |
| Shahan | 0:6893dd9a88b3 | 69 | r = rightspokecount; |
| Shahan | 0:6893dd9a88b3 | 70 | l = leftspokecount; |
| Shahan | 0:6893dd9a88b3 | 71 | while ((rightspokecount < rightspokecountrequired) || (leftspokecount < leftspokecountrequired)){ |
| Shahan | 0:6893dd9a88b3 | 72 | pc.printf("right count = %d, left count = %d",rightspokecount,leftspokecount); |
| Shahan | 0:6893dd9a88b3 | 73 | wait(0.4); |
| Shahan | 0:6893dd9a88b3 | 74 | } |
| Shahan | 0:6893dd9a88b3 | 75 | motors_stop(); |
| Shahan | 0:6893dd9a88b3 | 76 | wait(0.3); |
| Shahan | 0:6893dd9a88b3 | 77 | pc.printf("now re-calibrating\n\r"); |
| Shahan | 0:6893dd9a88b3 | 78 | if(r != l){ |
| Shahan | 0:6893dd9a88b3 | 79 | if((rightmotorspeed == 1)&&(leftmotorspeed == 1)){ |
| Shahan | 0:6893dd9a88b3 | 80 | if(r>l){ |
| Shahan | 0:6893dd9a88b3 | 81 | if((r - l) > 1){ |
| Shahan | 0:6893dd9a88b3 | 82 | rightmotorspeed = rightmotorspeed - 0.5; |
| Shahan | 0:6893dd9a88b3 | 83 | calibratemotors(); |
| Shahan | 0:6893dd9a88b3 | 84 | } |
| Shahan | 0:6893dd9a88b3 | 85 | } |
| Shahan | 0:6893dd9a88b3 | 86 | else if (r<l){ |
| Shahan | 0:6893dd9a88b3 | 87 | if((l - r) > 1){ |
| Shahan | 0:6893dd9a88b3 | 88 | leftmotorspeed = leftmotorspeed - 0.5; |
| Shahan | 0:6893dd9a88b3 | 89 | calibratemotors(); |
| Shahan | 0:6893dd9a88b3 | 90 | } |
| Shahan | 0:6893dd9a88b3 | 91 | } |
| Shahan | 0:6893dd9a88b3 | 92 | } |
| Shahan | 0:6893dd9a88b3 | 93 | else if(rightmotorspeed == 1){ |
| Shahan | 0:6893dd9a88b3 | 94 | if(r>l){ |
| Shahan | 0:6893dd9a88b3 | 95 | if((r - l) > 1){ |
| Shahan | 0:6893dd9a88b3 | 96 | leftmotorspeed = leftmotorspeed + (1 - leftmotorspeed)*0.5; |
| Shahan | 0:6893dd9a88b3 | 97 | calibratemotors(); |
| Shahan | 0:6893dd9a88b3 | 98 | } |
| Shahan | 0:6893dd9a88b3 | 99 | } |
| Shahan | 0:6893dd9a88b3 | 100 | else if (r<l){ |
| Shahan | 0:6893dd9a88b3 | 101 | if((l - r) > 1){ |
| Shahan | 0:6893dd9a88b3 | 102 | leftmotorspeed = leftmotorspeed - (1 - leftmotorspeed)*0.5; |
| Shahan | 0:6893dd9a88b3 | 103 | calibratemotors(); |
| Shahan | 0:6893dd9a88b3 | 104 | } |
| Shahan | 0:6893dd9a88b3 | 105 | } |
| Shahan | 0:6893dd9a88b3 | 106 | } |
| Shahan | 0:6893dd9a88b3 | 107 | else if(leftmotorspeed == 1){ |
| Shahan | 0:6893dd9a88b3 | 108 | if(r>l){ |
| Shahan | 0:6893dd9a88b3 | 109 | if((r - l) > 1){ |
| Shahan | 0:6893dd9a88b3 | 110 | rightmotorspeed = rightmotorspeed - (1 - rightmotorspeed)*0.5; |
| Shahan | 0:6893dd9a88b3 | 111 | calibratemotors(); |
| Shahan | 0:6893dd9a88b3 | 112 | } |
| Shahan | 0:6893dd9a88b3 | 113 | } |
| Shahan | 0:6893dd9a88b3 | 114 | else if (r<l){ |
| Shahan | 0:6893dd9a88b3 | 115 | if((l - r) > 1){ |
| Shahan | 0:6893dd9a88b3 | 116 | rightmotorspeed = rightmotorspeed + (1 - rightmotorspeed)*0.5; |
| Shahan | 0:6893dd9a88b3 | 117 | calibratemotors(); |
| Shahan | 0:6893dd9a88b3 | 118 | } |
| Shahan | 0:6893dd9a88b3 | 119 | } |
| Shahan | 0:6893dd9a88b3 | 120 | } |
| Shahan | 0:6893dd9a88b3 | 121 | } |
| Shahan | 0:6893dd9a88b3 | 122 | } |
| Shahan | 0:6893dd9a88b3 | 123 | |
| Shahan | 0:6893dd9a88b3 | 124 |