sources/app/speed/control.c

   1 #include "control.h"
   2 #include "image_prc.h"
   3
   4 struct control icontrol;
   5
   6 void motor_ctrl(unsigned int s, int direction){
   7
   8         if(s<0 || s>65535) s=0;
   9
  10         if(direction==0){
  11                 PWM1_SetRatio16(0);
  12                 PWM2_SetRatio16(s);
  13         }else{
  14                 PWM2_SetRatio16(0);
  15                 PWM1_SetRatio16(s);
  16         }
  17 }
  18
  19 void wplus_ctrl(unsigned int s){
  20
  21         WPLUS_SetRatio16(s);
  22 }
  23
  24 int control_pad(struct track *p){
  25         int x,y;
  26         unsigned int m=S0,w=MID,ms=S0,ws=MID;
  27         int i=0,j=0;
  28         int head=0;
  29         int trail=0;
  30
  31
  32         x=p[0].x;
  33         y=p[0].y;
  34
  35         if(x<15) ws=L0;
  36         if(x>35) ws=R0;
  37
  38         if(x==-1){
  39                 m=icontrol.m;
  40                 w=icontrol.w;
  41         }else{
  42                 while(p[i].x!=-1 && i<15){
  43                         head+=p[i++].vector;
  44                 }
  45                 head/=i;
  46                 if(i==10){
  47                         while(p[i].x!=-1 && i<20){
  48                                 trail+=p[i++].vector;
  49                                 j++;
  50                         }
  51                 trail=trail/j;
  52                 }
  53
  54
  55
  56
  57         switch (trail){
  58                 case 0:
  59                         m=S1;
  60                         break;
  61                 case 1:
  62                 case -1:
  63                         m=S1-SLOW;
  64                         break;
  65                 case 2:
  66                 case -2:
  67                 case 3:
  68                 case -3:
  69                         m=S1-2*SLOW;
  70                         break;
  71                 default:
  72                         m=S1-3*SLOW;
  73         }
  74         motor_ctrl(m,1);
  75
  76         switch(head){
  77                 case 0:
  78                         m=S1;
  79                       //  w=icontrol.w;
  80                         break;
  81                 case 1:
  82                         m=icontrol.m;
  83                         w=L1;
  84                         break;
  85                 case -1:
  86                         m=icontrol.m;
  87                         w=R1;
  88                         break;
  89                 case 2:
  90                 case 3:
  91                         m=icontrol.m-2*SLOW;
  92                         w=L2;
  93                         break;
  94                 case -2:
  95                 case -3:
  96                         m=icontrol.m-2*SLOW;
  97                         w=R2;
  98                         break;
  99                 case -4:
 100                 case -5:
 101                 case -6:
 102                 case -7:
 103                 case -8:
 104                 case -9:
 105                 case -10:
 106                 case -11:
 107                 case -12:
 108                         m=S0;
 109                         w=L3;
 110                         break;
 111                 case 4:
 112                 case 5:
 113                 case 6:
 114                 case 7:
 115                 case 8:
 116                 case 9:
 117                 case 10:
 118                 case 11:
 119                 case 12:
 120                         m=S0;
 121                         w=R3;
 122                         break;
 123                 default:
 124                         m=S0;
 125                         w=MID;
 126         }
 127          }
 128
 129         motor_ctrl(m,1);
 130         if(w<MID && w>ws) w=ws;
 131         if(w>MID && w<ws) w=ws;
 132
 133         wplus_ctrl(w);
 134         icontrol.m=m;
 135         icontrol.w=w;
 136
 137
 138         return 0;
 139 }