Revision 772
having issues, thinking of giving up on velocity thing
| branches/encoders/code/behaviors/encoder_test/main.c | ||
|---|---|---|
| 18 | 18 |
|
| 19 | 19 |
int main(void) |
| 20 | 20 |
{
|
| 21 |
int l=0,r=0, motorl=0, motorr=0; |
|
| 21 |
//int l=0,r=0, motorl=0, motorr=0, Il=0, Ir=0; |
|
| 22 |
|
|
| 23 |
unsigned int l=0, r=0; |
|
| 24 |
|
|
| 25 |
int motorl=0, motorr=0; |
|
| 22 | 26 |
|
| 23 | 27 |
short int lmin=0,rmin=0; |
| 24 | 28 |
|
| ... | ... | |
| 40 | 44 |
|
| 41 | 45 |
//calibration code: |
| 42 | 46 |
//TODO: this only find the min for stop->go, which is different than go->stop |
| 43 |
|
|
| 47 |
/* |
|
| 44 | 48 |
while(l<10){
|
| 45 | 49 |
l=encoder_get_dx(LEFT); |
| 46 | 50 |
motor1_set(FORWARD,++lmin); |
| ... | ... | |
| 59 | 63 |
|
| 60 | 64 |
usb_puts("lmin: ");usb_puti(lmin);
|
| 61 | 65 |
usb_puts(" rmin: ");usb_puti(rmin);usb_puts("\r\n");
|
| 66 |
|
|
| 67 |
button1_wait(); |
|
| 68 |
*/ |
|
| 69 |
lmin=161; |
|
| 70 |
rmin=161; |
|
| 62 | 71 |
|
| 63 |
|
|
| 64 | 72 |
//straight line forever code: |
| 65 |
|
|
| 73 |
/* |
|
| 66 | 74 |
motorl=lmin; |
| 67 | 75 |
motorr=rmin; |
| 68 | 76 |
|
| ... | ... | |
| 70 | 78 |
l=encoder_get_v(LEFT); |
| 71 | 79 |
r=encoder_get_v(RIGHT); |
| 72 | 80 |
|
| 73 |
motorl += (TARGET_V - l)*2;
|
|
| 74 |
motorr += (TARGET_V - r)*2;
|
|
| 81 |
motorl += (TARGET_V - l)*1;
|
|
| 82 |
motorr += (TARGET_V - r)*1;
|
|
| 75 | 83 |
|
| 76 |
usb_puti(motorl);usb_puts(":");usb_puti(l);usb_puts(" ");
|
|
| 77 |
usb_puti(motorr);usb_puts(":");usb_puti(r);usb_puts("\r\n");
|
|
| 84 |
Il += (TARGET_V - l);
|
|
| 85 |
Ir += (TARGET_V - r); //worked with +???
|
|
| 78 | 86 |
|
| 87 |
motorl += Il / 2; |
|
| 88 |
motorr += Ir / 2; |
|
| 89 |
|
|
| 90 |
usb_puti(l);usb_puts(":");usb_puti(motorl);usb_puts(" i: ");usb_puti(Il);usb_puts(" || ");
|
|
| 91 |
usb_puti(r);usb_puts(":");usb_puti(motorr);usb_puts(" i: ");usb_puti(Ir);usb_puts("\r\n");
|
|
| 92 |
|
|
| 79 | 93 |
if(motorl<0) |
| 80 | 94 |
motorl=0; |
| 81 | 95 |
if(motorl>255) |
| ... | ... | |
| 89 | 103 |
motor1_set(FORWARD,motorl); |
| 90 | 104 |
motor2_set(FORWARD,motorr); |
| 91 | 105 |
|
| 92 |
encoder_wait(1);
|
|
| 106 |
encoder_wait(5);
|
|
| 93 | 107 |
|
| 108 |
}*/ |
|
| 109 |
|
|
| 110 |
//straight line v2 |
|
| 111 |
|
|
| 112 |
l=0; |
|
| 113 |
r=0; |
|
| 114 |
|
|
| 115 |
lmin+=lmin/16; |
|
| 116 |
rmin+=rmin/16; |
|
| 117 |
|
|
| 118 |
motorl=0; |
|
| 119 |
motorr=0; |
|
| 120 |
|
|
| 121 |
while(1){
|
|
| 122 |
l = encoder_get_dx(LEFT); |
|
| 123 |
r = encoder_get_dx(RIGHT); |
|
| 124 |
|
|
| 125 |
if(l < r) |
|
| 126 |
motorl++; |
|
| 127 |
else |
|
| 128 |
motorr++; |
|
| 129 |
|
|
| 130 |
if(motorl>motorr){
|
|
| 131 |
motorl-=motorr; |
|
| 132 |
motorr=0; |
|
| 133 |
} |
|
| 134 |
else if(r<l){
|
|
| 135 |
r-=l; |
|
| 136 |
l=0; |
|
| 137 |
} |
|
| 138 |
|
|
| 139 |
usb_puti(l);usb_puts(":");usb_puti(motorl);usb_puts(" ");
|
|
| 140 |
usb_puti(r);usb_puts(":");usb_puti(motorr);usb_puts("\r\n");
|
|
| 141 |
|
|
| 142 |
motor1_set(FORWARD,lmin+motorl); |
|
| 143 |
motor2_set(FORWARD,rmin+motorr); |
|
| 144 |
|
|
| 145 |
encoder_wait(10); |
|
| 94 | 146 |
} |
| 95 | 147 |
|
| 96 | 148 |
|
Also available in: Unified diff