MoBot/MobotCode.ino at master · snibo13/MoBot · 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
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
//Library for motor
#include <SparkFun_TB6612.h>


//Error fixing
int TODO = 5;


//Pin assignment (Line follower)
const int rightSensor = TODO;
const int leftSensor = TODO;
const int midSensor = TODO;


const int lineLvl = TODO;

//Pin assignment (Motor)
int rightMotor1 = 2;
int rightMotor2 = 4;
int rightMotorVel = 5;

int leftMotor1 = 7;
int leftMotor2 = 8;
int leftMotorVel = 6;

int stdby = 9;

//Compensates for physical orientation and motor spin direction
int offsetL = 1;
int offsetR = 1;

Motor leftMotor = Motor(leftMotor1, leftMotor2, leftMotorVel, offsetL, stdby);
Motor rightMotor = Motor(rightMotor1, rightMotor2, rightMotorVel, offsetR, stdby);

//Physical variables
const float sensorOffset = 5;
const float radius = 5;
const float velocity = 50;
const int angle = 0;

//Pin assignment (Ultrasonic)
int triggerPin = TODO;
int echoPin = TODO;

//Ultrasonic variables
long duration;
int distance;


void setup() {
  pinMode(rightSensor, INPUT);
  pinMode(leftSensor, INPUT);
  pinMode(midSensor, INPUT);
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
  Serial.begin(9600);
}

void loop() {
  turn(0);
  delay(1000);
  leftMotor.brake();
  rightMotor.brake();
  delay(1000);
  turn(20);
  delay(1000);
  leftMotor.brake();
  rightMotor.brake();
  delay(1000);
  turn(-20);
  delay(1000);
  leftMotor.brake();
  rightMotor.brake();
  delay(1000);
}

void readLine() {
  float left = analogRead(leftSensor);
  float right = analogRead(rightSensor);
  float middle = analogRead(midSensor);
  if (left > lineLvl) {

  } else if (right >lineLvl) {

  }
}

/*
 * Turns the bot to the desired angle from the current
 * NOTE: Straightening requires a turn
 * param(theta): angle to turn to left or right
 */
void turn(float theta) {
  float t = sensorOffset/velocity;
  Serial.println(t);
  float w = theta/t;
  Serial.println(theta);
  float leftWheelVelocity = velocity - w;
  float rightWheelVelocity = velocity + w;
  Serial.println(leftWheelVelocity);
  Serial.println(rightWheelVelocity);
  if (leftWheelVelocity == rightWheelVelocity == 0) {
    leftMotor.brake();
    rightMotor.brake();
  }
  driveBot(leftWheelVelocity, rightWheelVelocity);
}

/*
 * Writes the voltages necessary to drive the left and
 * right wheels at the desired velocity
 * param leftWheelVelocity, rightWheelvelocity
 */
void driveBot(float lVelocity, float rVelocity) {
    leftMotor.drive(lVelocity, 1000);
    rightMotor.drive(rVelocity, 1000);

}
void readDistance() {

}