Commit ab0e452a by mehtank

motors

1 parent a2f170c7
......@@ -51,6 +51,19 @@
const int SERVO_LEFT = D1;
const int SERVO_RIGHT = D2;
const int MOTOR_PWM_LEFT = D1;
const int MOTOR_PWM_RIGHT = D2;
const int MOTOR_DIR_LEFT = D3;
const int MOTOR_DIR_RIGHT = D4;
int motor_left_flip = false;
int motor_right_flip = false;
#define MOTOR_BACK LOW
#define MOTOR_FWD HIGH
Servo servo_left;
Servo servo_right;
int servo_left_ctr = 90;
......@@ -114,13 +127,29 @@ void loop() {
//
void drive(int left, int right) {
#ifdef CRSERVO
servo_left.write(left);
servo_right.write(right);
#else
left -= 90;
left = (left * 34) / 3;
digitalWrite(MOTOR_DIR_LEFT, (left > 0) ^ motor_left_flip);
analogWrite(MOTOR_PWM_LEFT, abs(left));
right -= 90;
right = (right * 34) / 3;
digitalWrite(MOTOR_DIR_RIGHT, (right > 0) ^ motor_right_flip);
analogWrite(MOTOR_PWM_RIGHT, abs(right));
#endif
}
void stop() {
DEBUG("stop");
#ifdef CRSERVO
drive(servo_left_ctr, servo_right_ctr);
#else
drive(90, 90);
#endif
LED_OFF;
}
......@@ -159,8 +188,23 @@ void setupPins() {
LED_OFF; //Turn off LED
DEBUG("Setup LED pin.");
#ifdef CRSERVO
servo_left.attach(SERVO_LEFT);
servo_right.attach(SERVO_RIGHT);
#else
pinMode(MOTOR_PWM_LEFT, OUTPUT);
pinMode(MOTOR_DIR_LEFT, OUTPUT);
pinMode(MOTOR_PWM_RIGHT, OUTPUT);
pinMode(MOTOR_DIR_RIGHT, OUTPUT);
// Set initial speed to 0
analogWrite(MOTOR_PWM_LEFT, 0);
analogWrite(MOTOR_PWM_RIGHT, 0);
digitalWrite(MOTOR_DIR_LEFT, MOTOR_FWD ^ motor_left_flip);
digitalWrite(MOTOR_DIR_RIGHT, MOTOR_FWD ^ motor_right_flip);
#endif
DEBUG("Setup motor pins");
}
......@@ -208,19 +252,25 @@ void webSocketEvent(uint8_t id, WStype_t type, uint8_t * payload, size_t length)
else if(payload[1] == 'R')
right();
else if(payload[1] == 'U') {
if(payload[2] == 'L')
if(payload[2] == 'L') {
servo_left_ctr -= 1;
else if(payload[2] == 'R')
motor_left_flip = !motor_left_flip;
} else if(payload[2] == 'R') {
servo_right_ctr += 1;
motor_right_flip = !motor_right_flip;
}
char tx[20] = "Zero @ (xxx, xxx)";
sprintf(tx, "Zero @ (%3d, %3d)", servo_left_ctr, servo_right_ctr);
wsSend(id, tx);
}
else if(payload[1] == 'D') {
if(payload[2] == 'L')
if(payload[2] == 'L') {
servo_left_ctr += 1;
else if(payload[2] == 'R')
motor_left_flip = !motor_left_flip;
} else if(payload[2] == 'R') {
servo_right_ctr -= 1;
motor_right_flip = !motor_right_flip;
}
char tx[20] = "Zero @ (xxx, xxx)";
sprintf(tx, "Zero @ (%3d, %3d)", servo_left_ctr, servo_right_ctr);
wsSend(id, tx);
......
Styling with Markdown is supported
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!