Colony

dist_l: 0
+dist_r: 0
+
+
+
+speed is: 180  speed_l  156  speed_r  134
+speed is: 181  speed_l  354  speed_r  308
+speed is: 182  speed_l  558  speed_r  488
+speed is: 183  speed_l  761  speed_r  667
+speed is: 184  speed_l  981  speed_r  854
+speed is: 185  speed_l  1197  speed_r  1036
+speed is: 186  speed_l  1423  speed_r  1221
+speed is: 187  speed_l  1651  speed_r  1416
+speed is: 188  speed_l  1887  speed_r  1620
+speed is: 189  speed_l  2132  speed_r  1833
+speed is: 190  speed_l  2378  speed_r  2044
+speed is: 191  speed_l  2631  speed_r  2254
+speed is: 192  speed_l  2886  speed_r  2475
+speed is: 193  speed_l  3154  speed_r  2698
+speed is: 194  speed_l  3421  speed_r  2928
+speed is: 195  speed_l  3694  speed_r  3152
+speed is: 196  speed_l  3975  speed_r  3385
+speed is: 197  speed_l  4258  speed_r  3644
+speed is: 198  speed_l  4549  speed_r  3903
+speed is: 199  speed_l  4836  speed_r  4156
+speed is: 200  speed_l  5142  speed_r  4412
+speed is: 201  speed_l  5441  speed_r  4676
+speed is: 202  speed_l  5746  speed_r  4949
+speed is: 203  speed_l  6060  speed_r  5214
+speed is: 204  speed_l  6374  speed_r  5484
+speed is: 205  speed_l  6699  speed_r  5779
+speed is: 206  speed_l  7028  speed_r  6063
+speed is: 207  speed_l  7360  speed_r  6342
+speed is: 208  speed_l  7699  speed_r  6630
+speed is: 209  speed_l  8042  speed_r  6932
+speed is: 210  speed_l  8390  speed_r  7237
+speed is: 211  speed_l  8746  speed_r  7540
+speed is: 212  speed_l  9103  speed_r  7848
+speed is: 213  speed_l  9458  speed_r  8161
+speed is: 214  speed_l  9814  speed_r  8472
+speed is: 215  speed_l  10179  speed_r  8798
+speed is: 216  speed_l  10540  speed_r  9126
+speed is: 217  speed_l  10902  speed_r  9452
+speed is: 218  speed_l  11275  speed_r  9783
+speed is: 219  speed_l  11650  speed_r  10122
+speed is: 220  speed_l  12026  speed_r  10457
+speed is: 221  speed_l  12418  speed_r  10804
+speed is: 222  speed_l  12810  speed_r  11166
+speed is: 223  speed_l  13203  speed_r  11524
+speed is: 224  speed_l  13596  speed_r  11887
+speed is: 225  speed_l  13992  speed_r  12262
+speed is: 226  speed_l  14398  speed_r  12632
+speed is: 227  speed_l  14798  speed_r  13020
+speed is: 228  speed_l  15199  speed_r  13406
+speed is: 229  speed_l  15601  speed_r  13791
+speed is: 230  speed_l  16001  speed_r  14193
+speed is: 231  speed_l  16410  speed_r  14592
+speed is: 232  speed_l  16820  speed_r  15001
+speed is: 233  speed_l  17233  speed_r  15410
+speed is: 234  speed_l  17647  speed_r  15829
+speed is: 235  speed_l  18061  speed_r  16261
+speed is: 236  speed_l  18484  speed_r  16684
+speed is: 237  speed_l  18905  speed_r  17118
+speed is: 238  speed_l  19337  speed_r  17546
+speed is: 239  speed_l  19777  speed_r  17983
+speed is: 240  speed_l  20217  speed_r  18430
+speed is: 241  speed_l  20665  speed_r  18874
+speed is: 242  speed_l  21110  speed_r  19324
+speed is: 243  speed_l  21560  speed_r  19770
+speed is: 244  speed_l  22014  speed_r  20233
+speed is: 245  speed_l  22476  speed_r  20685
+speed is: 246  speed_l  22932  speed_r  21145
+speed is: 247  speed_l  23393  speed_r  21607
+speed is: 248  speed_l  23849  speed_r  22078
+speed is: 249  speed_l  24308  speed_r  22556
+speed is: 250  speed_l  24771  speed_r  23029
+speed is: 251  speed_l  25230  speed_r  23512
+speed is: 252  speed_l  25693  speed_r  23987
+speed is: 253  speed_l  26156  speed_r  24472
+speed is: 254  speed_l  26644  speed_r  24948
+speed is: 255  speed_l  27139  speed_r  25434

dist_l: 0
+dist_r: 1
+
+
+
+speed is: 180  speed_l  156  speed_r  145
+speed is: 181  speed_l  353  speed_r  328
+speed is: 182  speed_l  549  speed_r  518
+speed is: 183  speed_l  745  speed_r  718
+speed is: 184  speed_l  953  speed_r  929
+speed is: 185  speed_l  1170  speed_r  1146
+speed is: 186  speed_l  1391  speed_r  1373
+speed is: 187  speed_l  1610  speed_r  1602
+speed is: 188  speed_l  1832  speed_r  1838
+speed is: 189  speed_l  2068  speed_r  2081
+speed is: 190  speed_l  2312  speed_r  2332
+speed is: 191  speed_l  2556  speed_r  2586
+speed is: 192  speed_l  2796  speed_r  2843
+speed is: 193  speed_l  3054  speed_r  3105
+speed is: 194  speed_l  3316  speed_r  3374
+speed is: 195  speed_l  3577  speed_r  3646
+speed is: 196  speed_l  3834  speed_r  3921
+speed is: 197  speed_l  4112  speed_r  4203
+speed is: 198  speed_l  4393  speed_r  4495
+speed is: 199  speed_l  4672  speed_r  4782
+speed is: 200  speed_l  4955  speed_r  5075
+speed is: 201  speed_l  5258  speed_r  5379
+speed is: 202  speed_l  5558  speed_r  5687
+speed is: 203  speed_l  5854  speed_r  5999
+speed is: 204  speed_l  6165  speed_r  6309
+speed is: 205  speed_l  6488  speed_r  6629
+speed is: 206  speed_l  6805  speed_r  6944
+speed is: 207  speed_l  7127  speed_r  7263
+speed is: 208  speed_l  7459  speed_r  7597
+speed is: 209  speed_l  7799  speed_r  7932
+speed is: 210  speed_l  8146  speed_r  8268
+speed is: 211  speed_l  8495  speed_r  8614
+speed is: 212  speed_l  8841  speed_r  8959
+speed is: 213  speed_l  9190  speed_r  9303
+speed is: 214  speed_l  9547  speed_r  9656
+speed is: 215  speed_l  9899  speed_r  10007
+speed is: 216  speed_l  10260  speed_r  10362
+speed is: 217  speed_l  10633  speed_r  10734
+speed is: 218  speed_l  11006  speed_r  11102
+speed is: 219  speed_l  11389  speed_r  11477
+speed is: 220  speed_l  11770  speed_r  11860
+speed is: 221  speed_l  12142  speed_r  12246
+speed is: 222  speed_l  12536  speed_r  12632
+speed is: 223  speed_l  12924  speed_r  13017
+speed is: 224  speed_l  13317  speed_r  13404
+speed is: 225  speed_l  13712  speed_r  13809
+speed is: 226  speed_l  14099  speed_r  14214
+speed is: 227  speed_l  14513  speed_r  14615
+speed is: 228  speed_l  14925  speed_r  15021
+speed is: 229  speed_l  15337  speed_r  15423
+speed is: 230  speed_l  15752  speed_r  15834
+speed is: 231  speed_l  16161  speed_r  16250
+speed is: 232  speed_l  16579  speed_r  16672
+speed is: 233  speed_l  16994  speed_r  17099
+speed is: 234  speed_l  17410  speed_r  17525
+speed is: 235  speed_l  17842  speed_r  17957
+speed is: 236  speed_l  18274  speed_r  18394
+speed is: 237  speed_l  18720  speed_r  18843
+speed is: 238  speed_l  19157  speed_r  19289
+speed is: 239  speed_l  19604  speed_r  19449
+speed is: 240  speed_l  20046  speed_r  19456
+speed is: 241  speed_l  20488  speed_r  19466
+speed is: 242  speed_l  20931  speed_r  19471
+speed is: 243  speed_l  21370  speed_r  19477
+speed is: 244  speed_l  21820  speed_r  19384
+speed is: 245  speed_l  22266  speed_r  19398
+speed is: 246  speed_l  22724  speed_r  19279
+speed is: 247  speed_l  23173  speed_r  19275
+speed is: 248  speed_l  23630  speed_r  19250
+speed is: 249  speed_l  24086  speed_r  19244
+speed is: 250  speed_l  24539  speed_r  19348
+speed is: 251  speed_l  25014  speed_r  19345
+speed is: 252  speed_l  25499  speed_r  19415
+speed is: 253  speed_l  25993  speed_r  19380
+speed is: 254  speed_l  26482  speed_r  19432
+speed is: 255  speed_l  26978  speed_r  19402dist_l: -1
+dist_r: 0
+
+
+
+speed is: 180  speed_l  133  speed_r  141
+speed is: 181  speed_l  317  speed_r  323
+speed is: 182  speed_l  501  speed_r  515
+speed is: 183  speed_l  700  speed_r  704
+speed is: 184  speed_l  908  speed_r  904
+speed is: 185  speed_l  1120  speed_r  1108
+speed is: 186  speed_l  1332  speed_r  1319
+speed is: 187  speed_l  1544  speed_r  1540
+speed is: 188  speed_l  1772  speed_r  1755
+speed is: 189  speed_l  2006  speed_r  1982
+speed is: 190  speed_l  2237  speed_r  2213
+speed is: 191  speed_l  2474  speed_r  2451
+speed is: 192  speed_l  2715  speed_r  2692
+speed is: 193  speed_l  2970  speed_r  2939
+speed is: 194  speed_l  3226  speed_r  3191
+speed is: 195  speed_l  3480  speed_r  3448
+speed is: 196  speed_l  3740  speed_r  3714
+speed is: 197  speed_l  4015  speed_r  3981
+speed is: 198  speed_l  4290  speed_r  4250
+speed is: 199  speed_l  4562  speed_r  4530
+speed is: 200  speed_l  4842  speed_r  4809
+speed is: 201  speed_l  5133  speed_r  5096
+speed is: 202  speed_l  5426  speed_r  5384
+speed is: 203  speed_l  5714  speed_r  5681
+speed is: 204  speed_l  6019  speed_r  5974
+speed is: 205  speed_l  6325  speed_r  6274
+speed is: 206  speed_l  6625  speed_r  6582
+speed is: 207  speed_l  6943  speed_r  6893
+speed is: 208  speed_l  7264  speed_r  7208
+speed is: 209  speed_l  7581  speed_r  7521
+speed is: 210  speed_l  7904  speed_r  7845
+speed is: 211  speed_l  8235  speed_r  8169
+speed is: 212  speed_l  8560  speed_r  8500
+speed is: 213  speed_l  8894  speed_r  8841
+speed is: 214  speed_l  9244  speed_r  9178
+speed is: 215  speed_l  9588  speed_r  9518
+speed is: 216  speed_l  9936  speed_r  9869
+speed is: 217  speed_l  10291  speed_r  10217
+speed is: 218  speed_l  10642  speed_r  10574
+speed is: 219  speed_l  11000  speed_r  10941
+speed is: 220  speed_l  11375  speed_r  11309
+speed is: 221  speed_l  11746  speed_r  11680
+speed is: 222  speed_l  12128  speed_r  12050
+speed is: 223  speed_l  12507  speed_r  12430
+speed is: 224  speed_l  12879  speed_r  12817
+speed is: 225  speed_l  13263  speed_r  13201
+speed is: 226  speed_l  13652  speed_r  13591
+speed is: 227  speed_l  14042  speed_r  13994
+speed is: 228  speed_l  14435  speed_r  14400
+speed is: 229  speed_l  14822  speed_r  14804
+speed is: 230  speed_l  15221  speed_r  15211
+speed is: 231  speed_l  15633  speed_r  15616
+speed is: 232  speed_l  16042  speed_r  16035
+speed is: 233  speed_l  16458  speed_r  16455
+speed is: 234  speed_l  16866  speed_r  16877
+speed is: 235  speed_l  17284  speed_r  17303
+speed is: 236  speed_l  17697  speed_r  17726
+speed is: 237  speed_l  18110  speed_r  18153
+speed is: 238  speed_l  18534  speed_r  18585
+speed is: 239  speed_l  18959  speed_r  19023
+speed is: 240  speed_l  19394  speed_r  19461
+speed is: 241  speed_l  19823  speed_r  19905
+speed is: 242  speed_l  20255  speed_r  20347
+speed is: 243  speed_l  20694  speed_r  20797
+speed is: 244  speed_l  21130  speed_r  21255
+speed is: 245  speed_l  21576  speed_r  21717
+speed is: 246  speed_l  22012  speed_r  22187
+speed is: 247  speed_l  22460  speed_r  22654
+speed is: 248  speed_l  22902  speed_r  23128
+speed is: 249  speed_l  23348  speed_r  23602
+speed is: 250  speed_l  23804  speed_r  24080
+speed is: 251  speed_l  24257  speed_r  24558
+speed is: 252  speed_l  24714  speed_r  25041
+speed is: 253  speed_l  25166  speed_r  25524
+speed is: 254  speed_l  25651  speed_r  26006
+speed is: 255  speed_l  26137  speed_r  26493

dist_l: 0
+dist_r: 1
+
+
+
+speed is: 180  speed_l  158  speed_r  163
+speed is: 181  speed_l  354  speed_r  362
+speed is: 182  speed_l  560  speed_r  565
+speed is: 183  speed_l  772  speed_r  774
+speed is: 184  speed_l  991  speed_r  984
+speed is: 185  speed_l  1208  speed_r  1201
+speed is: 186  speed_l  1435  speed_r  1431
+speed is: 187  speed_l  1669  speed_r  1664
+speed is: 188  speed_l  1913  speed_r  1903
+speed is: 189  speed_l  2157  speed_r  2146
+speed is: 190  speed_l  2404  speed_r  2402
+speed is: 191  speed_l  2661  speed_r  2663
+speed is: 192  speed_l  2926  speed_r  2928
+speed is: 193  speed_l  3191  speed_r  3193
+speed is: 194  speed_l  3462  speed_r  3474
+speed is: 195  speed_l  3739  speed_r  3754
+speed is: 196  speed_l  4028  speed_r  4037
+speed is: 197  speed_l  4315  speed_r  4326
+speed is: 198  speed_l  4609  speed_r  4624
+speed is: 199  speed_l  4909  speed_r  4926
+speed is: 200  speed_l  5216  speed_r  5223
+speed is: 201  speed_l  5526  speed_r  5534p
+speed is: 202  speed_l  5837  speed_r  5849
+speed is: 203  speed_l  6157  speed_r  6166
+speed is: 204  speed_l  6477  speed_r  6490
+speed is: 205  speed_l  6805  speed_r  6818
+speed is: 206  speed_l  7144  speed_r  7153
+speed is: 207  speed_l  7482  speed_r  7495
+speed is: 208  speed_l  7821  speed_r  7837
+speed is: 209  speed_l  8172  speed_r  8182
+speed is: 210  speed_l  8523  speed_r  8533
+speed is: 211  speed_l  8880  speed_r  8889
+speed is: 212  speed_l  9248  speed_r  9251
+speed is: 213  speed_l  9617  speed_r  9620
+speed is: 214  speed_l  9988  speed_r  9990
+speed is: 215  speed_l  10365  speed_r  10360
+speed is: 216  speed_l  10745  speed_r  10739
+speed is: 217  speed_l  11133  speed_r  11115
+speed is: 218  speed_l  11528  speed_r  11492
+speed is: 219  speed_l  11928  speed_r  11882
+speed is: 220  speed_l  12336  speed_r  12277
+speed is: 221  speed_l  12746  speed_r  12681
+speed is: 222  speed_l  13155  speed_r  13086
+speed is: 223  speed_l  13566  speed_r  13493
+speed is: 224  speed_l  13986  speed_r  13904
+speed is: 225  speed_l  14412  speed_r  14313
+speed is: 226  speed_l  14839  speed_r  14728
+speed is: 227  speed_l  15275  speed_r  15141
+speed is: 228  speed_l  15712  speed_r  15557
+speed is: 229  speed_l  16155  speed_r  15993
+speed is: 230  speed_l  16601  speed_r  16426
+speed is: 231  speed_l  17048  speed_r  16868
+speed is: 232  speed_l  17505  speed_r  17309
+speed is: 233  speed_l  17963  speed_r  17755
+speed is: 234  speed_l  18426  speed_r  18202
+speed is: 235  speed_l  18892  speed_r  18646
+speed is: 236  speed_l  19357  speed_r  19098
+speed is: 237  speed_l  19826  speed_r  19546
+speed is: 238  speed_l  20298  speed_r  20001
+speed is: 239  speed_l  20771  speed_r  20449
+speed is: 240  speed_l  21244  speed_r  20910
+speed is: 241  speed_l  21721  speed_r  21387
+speed is: 242  speed_l  22202  speed_r  21869
+speed is: 243  speed_l  22686  speed_r  22350
+speed is: 244  speed_l  23171  speed_r  22833
+speed is: 245  speed_l  23660  speed_r  23322
+speed is: 246  speed_l  24149  speed_r  23813
+speed is: 247  speed_l  24646  speed_r  24309
+speed is: 248  speed_l  25145  speed_r  24804
+speed is: 249  speed_l  25664  speed_r  25302
+speed is: 250  speed_l  26181  speed_r  25799
+speed is: 251  speed_l  26705  speed_r  26300
+speed is: 252  speed_l  27229  speed_r  26798
+speed is: 253  speed_l  27757  speed_r  27301
+speed is: 254  speed_l  28285  speed_r  27799
+speed is: 255  speed_l  28818  speed_r  28302

dist_l: 0
+dist_r: 0
+
+
+
+speed is: 180  speed_l  94  speed_r  134
+speed is: 181  speed_l  228  speed_r  306
+speed is: 182  speed_l  380  speed_r  483
+speed is: 183  speed_l  518  speed_r  668
+speed is: 184  speed_l  659  speed_r  862
+speed is: 185  speed_l  812  speed_r  1056
+speed is: 186  speed_l  992  speed_r  1264
+speed is: 187  speed_l  1171  speed_r  1475
+speed is: 188  speed_l  1375  speed_r  1692
+speed is: 189  speed_l  1561  speed_r  1918
+speed is: 190  speed_l  1750  speed_r  2144
+speed is: 191  speed_l  1959  speed_r  2385
+speed is: 192  speed_l  2178  speed_r  2621
+speed is: 193  speed_l  2413  speed_r  2874
+speed is: 194  speed_l  2631  speed_r  3127
+speed is: 195  speed_l  2856  speed_r  3391
+speed is: 196  speed_l  3101  speed_r  3652
+speed is: 197  speed_l  3353  speed_r  3930
+speed is: 198  speed_l  3602  speed_r  4207
+speed is: 199  speed_l  3849  speed_r  4493
+speed is: 200  speed_l  4113  speed_r  4780
+speed is: 201  speed_l  4388  speed_r  5078
+speed is: 202  speed_l  4657  speed_r  5383
+speed is: 203  speed_l  4925  speed_r  5683
+speed is: 204  speed_l  5211  speed_r  5997
+speed is: 205  speed_l  5513  speed_r  6309
+speed is: 206  speed_l  5798  speed_r  6625
+speed is: 207  speed_l  6093  speed_r  6954
+speed is: 208  speed_l  6408  speed_r  7285
+speed is: 209  speed_l  6719  speed_r  7619
+speed is: 210  speed_l  7029  speed_r  7960
+speed is: 211  speed_l  7363  speed_r  8304
+speed is: 212  speed_l  7694  speed_r  8658
+speed is: 213  speed_l  8023  speed_r  9013
+speed is: 214  speed_l  8361  speed_r  9371
+speed is: 215  speed_l  8707  speed_r  9730
+speed is: 216  speed_l  9042  speed_r  10096
+speed is: 217  speed_l  9402  speed_r  10467
+speed is: 218  speed_l  9763  speed_r  10842
+speed is: 219  speed_l  10122  speed_r  11222
+speed is: 220  speed_l  10490  speed_r  11609
+speed is: 221  speed_l  10850  speed_r  11994
+speed is: 222  speed_l  11213  speed_r  12381
+speed is: 223  speed_l  11586  speed_r  12774
+speed is: 224  speed_l  11949  speed_r  13169
+speed is: 225  speed_l  12328  speed_r  13564
+speed is: 226  speed_l  12726  speed_r  13957
+speed is: 227  speed_l  13115  speed_r  14368
+speed is: 228  speed_l  13517  speed_r  14785
+speed is: 229  speed_l  13912  speed_r  15208
+speed is: 230  speed_l  14310  speed_r  15632
+speed is: 231  speed_l  14716  speed_r  16054
+speed is: 232  speed_l  15114  speed_r  16479
+speed is: 233  speed_l  15517  speed_r  16908
+speed is: 234  speed_l  15921  speed_r  17339
+speed is: 235  speed_l  16325  speed_r  17772
+speed is: 236  speed_l  16765  speed_r  18206
+speed is: 237  speed_l  17191  speed_r  18657
+speed is: 238  speed_l  17630  speed_r  19115
+speed is: 239  speed_l  18065  speed_r  19579
+speed is: 240  speed_l  18499  speed_r  20046
+speed is: 241  speed_l  18941  speed_r  20513
+speed is: 242  speed_l  19379  speed_r  20985
+speed is: 243  speed_l  19828  speed_r  21456
+speed is: 244  speed_l  20264  speed_r  21929
+speed is: 245  speed_l  20713  speed_r  22402
+speed is: 246  speed_l  21153  speed_r  22877
+speed is: 247  speed_l  21598  speed_r  23351
+speed is: 248  speed_l  22047  speed_r  23826
+speed is: 249  speed_l  22492  speed_r  24303
+speed is: 250  speed_l  22957  speed_r  25332
+speed is: 251  speed_l  23436  speed_r  25473
+speed is: 252  speed_l  23929  speed_r  25289
+speed is: 253  speed_l  24413  speed_r  25481
+speed is: 254  speed_l  24907  speed_r  25256
+speed is: 255  speed_l  25393  speed_r  25489

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

int delta = 16;

int divisor = 7;

int epsilon = 8;

/*

  speed initial motor speed.

  distance in encoder_get_x unit.

  Has trouble when the distance difference is greater than epsilon * divisor

  better re initiate with encoders_init() since it might overflow. or with initial difference.

  @param speed 

  @param distance 30000 is roughly <10 meter

  @param time 

      not yet implemented.

 */

void encoder_censored_forward(int speed, int distance, int time){

	encoders_init();

	int dist_l = encoder_get_x(LEFT);

	usb_puts("dist_l: ");

	usb_puti(dist_l);

	usb_puts("\r");

	int dist_r = encoder_get_x(RIGHT);

	usb_puts("dist_l: ");

	usb_puti(dist_l);

	usb_puts("\r");

	motor_l_set(FORWARD, speed);

	motor_r_set(FORWARD, speed);

	int spd_l = speed;

	int spd_r = speed;

	int dist_diff = 0;

	// int dist_diff_new = 0;  // was used for ensure the new speed is helping

	int count = 0; // count for time

	while (dist_l<distance && dist_r<distance){

	  delay_ms(40);

	  count++;

	  dist_l = encoder_get_x(LEFT);

	  dist_r = encoder_get_x(RIGHT);

	  dist_diff = dist_l-dist_r;

	  if (dist_diff< -delta) {

	    spd_l += -dist_diff/divisor;

	    spd_l = (spd_l >speed+epsilon)? speed+epsilon : spd_l;

	    spd_r -= -dist_diff/divisor;

	    spd_r = (spd_r <speed-epsilon)? speed-epsilon : spd_r;

	  }else if (dist_diff > delta){

	    spd_l -= dist_diff/divisor;

	    spd_l = (spd_l <speed-epsilon)? speed-epsilon : spd_l;

	    spd_r += dist_diff/divisor;

	    spd_r = (spd_r >speed+epsilon)? speed+epsilon : spd_r;

	  }

	  motor_l_set(FORWARD, spd_l);

	  motor_r_set(FORWARD, spd_r);

	  usb_puts( "dist_l and dist_r: ");

	  usb_puti(dist_l);

	  usb_puts("   ");

	  usb_puti(dist_r);

	  usb_puts("\r");

	}

	motor_l_set(FORWARD, 0);

	motor_r_set(FORWARD, 0);

	usb_puts("time :");

	usb_puti(count *40);

} 

void test_encoder(void){

  	encoders_init();

	int dist_l = encoder_get_x(LEFT);

	usb_puts("dist_l: ");

	usb_puti(dist_l);

	usb_puts("\r");

	int dist_r = encoder_get_x(RIGHT);

	usb_puts("dist_r: ");

	usb_puti(dist_r);

	usb_puts("\r\r\r");

	for (int i=180;i<256; i++){

	  motor_l_set(FORWARD, i);

	  motor_r_set(FORWARD, i);

	  delay_ms(200);

	  usb_puts("\rspeed is: ");

	  usb_puti(i);

	  usb_puts("  speed_l  ");

	  usb_puti(encoder_get_dx(LEFT));

	  usb_puts("  speed_r  ");

	  usb_puti(encoder_get_dx(RIGHT));

	}

}

int main(void)

{

	/* Initialize dragonfly board */

    	dragonfly_init(ALL_ON);

    	/* Initialize the basic wireless library */

    	wl_basic_init_default();

    	/* Set the XBee channel to 24 - must be standard among robots */

	wl_set_channel(24);

	encoder_censored_forward(225, 30000, 3000);

	//	test_encoder();

	while(1); /* END HERE, just in case something happened.  This way we can see the red orb. */

}

# this is a local makefile

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = main

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

#AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile