#define FrontPotPIN A0 //Potentiometer Analog pin. The pot Must receive 3.3v not 5
#define BackPotPIN A1
#define Aux5PIN 25 //Top left adjustment- used for BackLeg movement. RED wire
#define Aux6PIN 24 // Top right adjustment - used forFront Leg
volatile uint16_t BackLegVal;
uint16_t BackLeg_start;
volatile uint16_t FrontLegVal;
uint16_t FrontLeg_start;
// Legs Motors Section
#define FrontPotPIN A0 //Potentiometer Analog pin. The pot Must receive 3.3v not 5 --Yellow long wire
#define BackPotPIN A1
const int FrontLegUpPin = 23; // (MotorA)Pin for FrontMotor going UP-// it means that 0 is max speed and 255 no current
const int FrontLegDnPin = 22; // Pin for FrontMotor Down
const int BackLegUpPin = 21; // (MotorB)Pin for right motor Forward move PWM
const int BackLegDnPin = 20; // Pin for right motor BACK move PWM
uint32_t FrontLegIntCnt = 0; // Count of interrupts
uint32_t BackLegIntCnt = 0; // Count of interrupts on Back leg
void setup() {
while (!Serial && (millis() < 4000)) ; // wait for serial port to be there
Serial.begin(9600);
delay(250);
Serial.println("/n/n********** Program Start *********\n"); // print seomething at start
//Legs Section
pinMode(FrontPotPIN, INPUT);
pinMode(BackPotPIN, INPUT);
pinMode(FrontLegUpPin, OUTPUT);
pinMode(BackLegUpPin, OUTPUT);
pinMode(FrontLegDnPin, OUTPUT);
pinMode(BackLegDnPin, OUTPUT);
attachInterrupt(Aux5PIN, BackLegRC_Read_Fun, CHANGE); //Top lef adjustment
attachInterrupt(Aux6PIN, FrontLegRC_Read_Fun, CHANGE);
}
void loop() {
/*
analogWrite (FrontLegUpPin, 256);
analogWrite (FrontLegDnPin, 200);
delay (2000);
analogWrite (FrontLegDnPin, 256);
analogWrite (FrontLegUpPin, 200);
delay (2000);
*/
uint32_t loop_start = millis();
FrontLegMotionFun();
uint32_t after_front = millis();
BackLegMotionFun();
uint32_t after_back = millis();
Serial.printf("DT front: %d back: %d INTS: %d %d\n", after_front - loop_start, after_back - after_front,
FrontLegIntCnt, BackLegIntCnt);
}
void FrontLegMotionFun ()
{
int FrontLegPotVal = (analogRead(FrontPotPIN) - 220); //first we read the pot position and we ad a number to adjust the "Center point"
int GapFront = (FrontLegPotVal - (FrontLegVal - 1100)); // we are using "-1100" so we're left with a clean value from 0 to 700
/*
Serial.print("FrontLegPotVal");
Serial.print(FrontLegPotVal);
Serial.print("\t");
Serial.print("FrontLegRC Input");
Serial.print(FrontLegVal);
Serial.print("\t");
Serial.print("GapFront=");
Serial.println(GapFront);
*/
if (GapFront <= -30)
{
digitalWrite (FrontLegUpPin, 1);
digitalWrite (FrontLegDnPin, 0); // pin22 white wire
}
else if (GapFront >= 30 && FrontLegVal > 0)
{
digitalWrite (FrontLegUpPin, 0);
digitalWrite (FrontLegDnPin, 1);
}
else // do nothing
{
digitalWrite (FrontLegUpPin, 1);
digitalWrite (FrontLegDnPin, 1);
}
}
void BackLegMotionFun ()
{
int BackLegPotVal = (analogRead(BackPotPIN) - 100); //first we read the pot position and we ad a number to adjust the "Center point"
int GapBack = (BackLegPotVal - (BackLegVal - 1100)); // we are using "-1100" so we're left with a clean value from 0 to 700
/*
Serial.print("BackLegPotVal=");
Serial.print(BackLegPotVal);
Serial.print("\t");
Serial.print("BackLegRC Input=");
Serial.print(BackLegVal);
Serial.print("\t");
Serial.print("GapBack=");
Serial.println(GapBack);
*/
if (GapBack <= -30 )
{
digitalWrite (BackLegUpPin, 1);
digitalWrite (BackLegDnPin, 0); // pin22 white wire
}
else if (GapBack >= 30 && BackLegPotVal > 60)
{
digitalWrite (BackLegUpPin, 0);
digitalWrite (BackLegDnPin, 1);
}
else // do nothing
{
digitalWrite (BackLegUpPin, 1);
digitalWrite (BackLegDnPin, 1);
}
}
void FrontLegRC_Read_Fun() { // reverse drive
FrontLegIntCnt++;
if (digitalRead(Aux6PIN) == HIGH) {
FrontLeg_start = micros();
}
else {
FrontLegVal = (uint16_t) (micros() - FrontLeg_start);
}
}
void BackLegRC_Read_Fun() { //
BackLegIntCnt++;
if (digitalRead(Aux5PIN) == HIGH) {
BackLeg_start = micros();
}
else {
BackLegVal = (uint16_t) (micros() - BackLeg_start);
}
}
