Commit 895fa203 authored by Beck-Sisyphus's avatar Beck-Sisyphus
Browse files

Merge drive moter, arm, and hand code

parent f6be0e40
......@@ -14,45 +14,46 @@ void setup() {
Serial1.begin(BAUD_RATE);
leftMotor.attach(motorPin1);
rightMotor.attach(motorPin2);
MotorStop();
// start the Ethernet and UDP:
Ethernet.begin(mac,ip);
Udp.begin(localPort);
// i2c
Wire.begin(4); // Join i2c with address #1
Wire.onReceive(receivei2c); // Call back
// set up the arm to steady
for(int i = 0; i < 3; i++) {
armMotor[i].attach(TALON_ARM[i]);
armMotor[i].writeMicroseconds(NEUTRAL_FREQUENCY);
}
Serial.println("Intialized");
}
void loop() {
// Pass in the pot value from 0 to 1023
driveMotorFullSpeed(1023);
// driveMotorFullSpeed(1023);
// getReadingFromCurrentSensor();
armRiseAndBack();
delay(100);
// armRiseAndBack();
// delay(100);
int networkStatus = processNetworkData();
if(networkStatus == 1) {
// Refresh timeout
timeLastPacket = millis();
// Process new values;
leftMotor.writeMicroseconds(map(leftPower, 0, 255, MIN_FREQUENCY, MAX_FREQUENCY));
rightMotor.writeMicroseconds(map(rightPower, 0, 255, MIN_FREQUENCY, MAX_FREQUENCY));
for(int i = 0; i < 4; i++) {
armMotor[i].writeMicroseconds(dirToMicroseconds(arm[i]));
}
for(int i = 0; i < 3; i++) {
Serial.print("Hand: ");
Serial.print(i);
Serial.println(hand[i]);
if(hand[i] == 0 || hand[i] == 2) {
if(hand[i] == 0 && handPos[i] < 180) {
handPos[i] += 5;
} else if(handPos[i] > 0) {
handPos[i] -= 5;
}
// handMotor[i].write(handPos[i]);
}
// Check timeout, disable if necessary
if(millis() - timeLastPacket >= TIMEOUT) {
//Serial.println("TIMEOUT");
leftMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
rightMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
} else {
int networkStatus = processNetworkData();
if(networkStatus == 1) {
// Refresh timeout
timeLastPacket = millis();
// Process new values;
driveMotor();
moveArm();
moveHand();
}
}
}
......
......@@ -3,19 +3,20 @@
// Dual motor, only forwards and backwards
#include <Arduino.h>
void driveMotorFullSpeed(int potValue) {
// For motor current testing
// void driveMotorFullSpeed(int potValue) {
void driveMotor() {
// PWM have pulse seconds from 1000 microseconds to 2000
int leftSignal = map(leftPower, 0, 255, MIN_FREQUENCY, MAX_FREQUENCY);
int rightSignal= map(rightPower, 0, 255, MIN_FREQUENCY, MAX_FREQUENCY);
if (leftSignal < 1480 || leftSignal > 1520 || rightSignal < 1480 || leftSignal > 1520) {
leftMotor.writeMicroseconds(leftSignal);
rightMotor.writeMicroseconds(-rightSignal);
} else {
MotorStop();
}
}
// PWM have pulse seconds from 1000 microseconds to 2000
sendValue = map(potValue, 0, 1023, 1250, 1750);
if (sendValue < 1480 || sendValue > 1520)
{
leftMotor.writeMicroseconds(sendValue);
rightMotor.writeMicroseconds(-sendValue);
} else {
leftMotor.writeMicroseconds(1500);
rightMotor.writeMicroseconds(1500);
}
// delay(10);
void MotorStop() {
leftMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
rightMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
}
......@@ -2,13 +2,24 @@
// Using Analog read from current sensor ACS758
#include <Arduino.h>
void armRiseAndBack() {
void moveArm() {
for(int i = 0; i < 4; i++) {
armMotor[i].writeMicroseconds(dirToMicroseconds(arm[i]));
}
}
void moveHand() {
for(int i = 0; i < 3; i++) {
armMotor[i].writeMicroseconds(SPEEDUP_FREQUENCY);
Serial.println(TALON_ARM[i]);
delay(5000);
armMotor[i].writeMicroseconds(SPEEDDOWN_FREWQUENCY);
delay(5000);
armMotor[i].writeMicroseconds(NEUTRAL_FREQUENCY);
Serial.print("Hand: ");
Serial.print(i);
Serial.println(hand[i]);
if(hand[i] == 0 || hand[i] == 2) {
if(hand[i] == 0 && handPos[i] < 180) {
handPos[i] += 5;
} else if(handPos[i] > 0) {
handPos[i] -= 5;
}
handMotor[i].write(handPos[i]);
}
}
}
......@@ -217,12 +217,7 @@ void setup() {
}
void loop() {
// Check timeout, disable if necessary
if(millis() - timeLastPacket >= TIMEOUT) {
//Serial.println("TIMEOUT");
leftMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
rightMotor.writeMicroseconds(NEUTRAL_FREQUENCY);
}
// Get sensor information
pressureValue= analogRead(PRESSURE) * 0.363;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment