DifferentialDriveSimulator-Processing/DiffDrive.pde at master · artica/DifferentialDriveSimulator-Processing · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
int delayCounter = 0;
int delayMaxCounter = (int)random(2000);
int diffDriveState = 0;

int motorSpeedLeftTemp = (int)random(-255, 255);
int motorSpeedRightTemp = (int)random(-255, 255);

float motorSpeedLeft,  motorSpeedRight;
float filter = 0.025;


void DiffDriveProgram()
{

  if (diffDriveState == 0)
  {
    if(delayCounter < diffDriveState)
    {
      delayCounter++;
       DiffDrive((int)motorSpeedLeft, (int)motorSpeedRight);
    }
    else
    {
      delayCounter=0;
      delayMaxCounter = (int)random(250);
      diffDriveState++;

      motorSpeedLeftTemp = (int)random(250);
      motorSpeedRightTemp = (int)random(250);


    }
  }

  if (diffDriveState == 1)
  {
    if(delayCounter < delayMaxCounter)
    {
      delayCounter++;
        DiffDrive((int)motorSpeedLeft, (int)motorSpeedRight);
    }
    else
    {
      delayCounter=0;
      delayMaxCounter = (int)random(1250);
      diffDriveState=0;

      motorSpeedLeftTemp = (int)random(-255, 255);
      motorSpeedRightTemp = (int)random(-255, 255);
    }
  }

  motorSpeedLeft = motorSpeedLeft * (1.0-filter) + motorSpeedLeftTemp * filter;
  motorSpeedRight = motorSpeedRight * (1.0-filter) + motorSpeedRightTemp * filter;

  println(motorSpeedLeft);

}


void DiffDrive(int _ml, int _mr)
{
  motorValLeft = (int)map(_ml, 255, -255, -50, 50);
  motorValRight = (int)map(_mr, 255, -255, -50, 50);
}