mitchellhansen
/
battletank
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '90b9831079'
${ noResults }
battletank/dozer_main.cpp

299 lines
9.7 KiB
Raw Blame History

#include <util/delay.h>
#include <avr/io.h>
#include <stdio.h>
#include <stdarg.h>
#include <util/setbaud.h>
#include <Arduino.h>
#include <lib.h>
#include <SoftPWM.h>
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#define CE_PIN 3
#define CS_PIN 2
#define RED 4
#define GREEN 5
class Radio {
public:
RF24 instance;
};
class Motor {
public:
int power;
};
const uint64_t dozer_pipe = 0xF0F0F0F0D2LL;
const uint64_t remote_pipe = 0xF0F0F0F0E1LL;
#pragma pack(8)
struct StickValues {
int16_t stick_1_y;
int16_t stick_1_x;
int16_t stick_2_y;
int16_t stick_2_x;
};
#define R_TRK_FWD 18
#define L_TRK_FWD 17
#define R_TRK_BKW 19
#define L_TRK_BKW 16
#define BKT_FWD 14
#define BKT_BKW 15
#define PI 3.141592654
#define DEG(rad) (rad*180.0/PI)
#define FULL_SPEED_ZONE 5
#define MAGIC_45_2_100 2.22222
#define MAG_SCALER_H 200
#define MAG_SCALER_L 290
int main() {
init();
initVariant();
SPI.begin();
Serial.begin(9600);
pinMode(GREEN, OUTPUT);
pinMode(RED, OUTPUT);
SoftPWMBegin();
RF24 radio(CE_PIN, CS_PIN);
radio.begin();
radio.setPALevel (RF24_PA_MAX);
radio.setChannel(100);
radio.setDataRate(RF24_1MBPS);
radio.openWritingPipe(dozer_pipe);
radio.openReadingPipe(1, remote_pipe);
radio.startListening();
bool breaking = false;
int cycles = 0;
while (!breaking) {
bool timeout = false;
while (!radio.available()) {
delay(5);
if (cycles++ > 10) {
// digitalWrite(RED, HIGH);
_delay_ms(1);
// digitalWrite(RED, LOW);
_delay_ms(1);
timeout = true;
cycles = 0;
break;
}
}
if (timeout) {
SoftPWMSet(14, 0); // bucket
SoftPWMSet(15, 0); // bucket
SoftPWMSet(16, 0); // left track backward
SoftPWMSet(17, 0); // left track forward
SoftPWMSet(19, 0); // right track backward
SoftPWMSet(18, 0); // right track forward
} else {
cycles = 0;
StickValues stick_values{};
if (radio.read(&stick_values, sizeof(int16_t)*4)) {
// digitalWrite(GREEN, HIGH);
// _delay_ms(5);
// digitalWrite(GREEN, LOW);
// _delay_ms(5);
//Serial.println((int16_t)stick_values.stick_1_y);
}
radio.flush_rx();
if (stick_values.stick_1_y > 10) {
digitalWrite(GREEN, HIGH);
digitalWrite(RED, LOW);
} else if (stick_values.stick_1_y < -10) {
digitalWrite(RED, HIGH);
digitalWrite(GREEN, LOW);
} else {
digitalWrite(RED, LOW);
digitalWrite(GREEN, LOW);
}
float mag = sqrt(pow(stick_values.stick_1_x, 2) + pow(stick_values.stick_1_y, 2));
float ang = DEG(atan2((float)stick_values.stick_1_x, (float)stick_values.stick_1_y)) + 180;
int right_track = 0;
int left_track = 0;
if (ang >= 0 && ang < 90 && mag > 1) { // Bottom to Left Quad. RT:F
if (ang < 0 + FULL_SPEED_ZONE)
right_track = (int)(-100.0 * (mag/MAG_SCALER_H));
else if (ang > 90 - FULL_SPEED_ZONE)
right_track = (int)(100.0 * (mag/MAG_SCALER_H));
else // split it (and mult) so it's ranged between -100, 100 then mag it
right_track = ((0 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);
left_track = (int)(-100.0 * (mag/MAG_SCALER_H));
}
if (ang >= 90 && ang < 180 && mag > 1) { // Top to Left Quad. RT:F, LT:S
right_track = (int)(100.0 * (mag/MAG_SCALER_H));
if (ang < 90 + FULL_SPEED_ZONE)
left_track = (int)(-100.0 * (mag/MAG_SCALER_H));
else if (ang > 180 - FULL_SPEED_ZONE)
left_track = (int)(100.0 * (mag/MAG_SCALER_H));
else
left_track = ((90 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);
}
if (ang >= 180 && ang < 270 && mag > 1) { // Top to Right Quad
if (ang < 180 + FULL_SPEED_ZONE)
right_track = (int)(100.0 * (mag/MAG_SCALER_H));
else if (ang > 270 - FULL_SPEED_ZONE)
right_track = (int)(-100.0 * (mag/MAG_SCALER_H));
else
right_track = ((180 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);
left_track = (int)(100.0 * (mag/MAG_SCALER_H));
}
if (ang >= 270 && ang <= 360 && mag > 1) { // Bottom to Right Quad
right_track = (int)(100.0 * (mag/MAG_SCALER_H)) * -1;
if (ang < 270 + FULL_SPEED_ZONE)
left_track = (int)(100.0 * (mag/MAG_SCALER_H));
else if (ang > 360 - FULL_SPEED_ZONE)
left_track = (int)(100.0 * (mag/MAG_SCALER_H)) * -1;
else
left_track = ((270 + 45) - ang) * MAGIC_45_2_100 * (mag/MAG_SCALER_H);
}
// if (ang >= 0 && ang < 90 && mag > 1) {
// if (ang < 0 + FULL_SPEED_ZONE)
// right_track = left_track = -100;
//// else if (ang > 90 - FULL_SPEED_ZONE)
//// right_track = left_track = -100;
// else
// right_track = ((270 + 45) - ang) * 2 * (mag/290);
//
// left_track = -100 * (int)(mag/290);
// }
// if (ang >= 90 && ang < 180 && mag > 1) {
// right_track = 100 * (mag/290);
// if (ang > 90 - FULL_SPEED_ZONE) {
// right_track = 100;
// left_track = -100;
// }
// else
// left_track = (135 - ang) * -2 * (mag/290);
// }
// if (ang >= 180 && ang < 270 && mag > 1) {
// if (ang > 180 - FULL_SPEED_ZONE)
// right_track = 100 * (mag/290);
// else
// right_track = ((180 + 45) - ang) * 2 * (mag/290);
// left_track = 100 * (mag/290);
// }
// if (ang >= 270 && ang <= 360 && mag > 1) {
// right_track = -100 * (mag/290);
// if (ang > 270 - FULL_SPEED_ZONE) {
// left_track = -100 * (mag/290);
// } else {
// left_track = ((270 + 45) - ang) * 2 * (mag/290);
// }
// }
if (left_track > 0) {
SoftPWMSet(L_TRK_FWD, max(0, min(255, left_track)));
} else if (left_track < 0) {
SoftPWMSet(L_TRK_BKW, max(0, min(255, abs(left_track))));
} else if (left_track == 0) {
SoftPWMSet(L_TRK_BKW, 0);
SoftPWMSet(L_TRK_FWD, 0);
}
if (right_track > 0) {
SoftPWMSet(R_TRK_FWD, max(0, min(255, right_track)));
} else if (right_track < 0) {
SoftPWMSet(R_TRK_BKW, max(0, min(255, abs(right_track))));
} else if (right_track == 0) {
SoftPWMSet(R_TRK_FWD, 0);
SoftPWMSet(R_TRK_BKW, 0);
}
// int forward_delta = 0;
// if (stick_values.stick_1_x > 10) {
// forward_delta = min(max(0, stick_values.stick_1_x), 255);
//
// } else if (stick_values.stick_1_x < -10) {
// forward_delta = min(max(-255, stick_values.stick_1_x), 0);
// }
//
// if (stick_values.stick_1_y > 10) {
// int delta = min(max(0, stick_values.stick_1_y), 255);
// if (forward_delta > 0) {
// SoftPWMSet(R_TRK_FWD, delta + forward_delta);
// } else if (forward_delta <= 0) {
// SoftPWMSet(R_TRK_BKW, delta + forward_delta);
// }
// SoftPWMSet(L_TRK_FWD, delta);
// } else if (stick_values.stick_1_y < -10) {
// int delta = min(max(0, abs(stick_values.stick_1_y)), 255);
// if (forward_delta > 0) {
// SoftPWMSet(L_TRK_FWD, delta + forward_delta);
// } else if (forward_delta <= 0) {
// SoftPWMSet(L_TRK_BKW, delta + forward_delta);
// }
// SoftPWMSet(R_TRK_FWD, delta);
// } else {
//
// if (forward_delta > 10) {
// SoftPWMSet(L_TRK_FWD, forward_delta);
// SoftPWMSet(R_TRK_FWD, forward_delta);
// } else if (forward_delta < 10) {
// SoftPWMSet(L_TRK_BKW, forward_delta);
// SoftPWMSet(R_TRK_BKW, forward_delta);
// } else {
// SoftPWMSet(R_TRK_FWD, 0);
// SoftPWMSet(R_TRK_BKW, 0);
// SoftPWMSet(L_TRK_BKW, 0);
// SoftPWMSet(L_TRK_FWD, 0);
// }
// }
if (serialEventRun) serialEventRun();
}
}
}
//SoftPWMSet(14, 255); // bucket
//SoftPWMSet(15, 10); // bucket
//SoftPWMSet(16, 255); // left track backward
//SoftPWMSet(17, 255); // left track forward
//SoftPWMSet(19, 255); // right track backward
//SoftPWMSet(18, 255); // right track forward
// SoftPWMSet(17, stick_values.stick_1_y);
// SoftPWMSet(18, stick_values.stick_1_x);
// SoftPWMSet(19, stick_values.stick_2_y);
// SoftPWMSet(20, stick_values.stick_2_x);
// SoftPWMSet(21, stick_values.stick_3_a);
// SoftPWMSet(22, stick_values.stick_3_b);
Reference in new issue View Git Blame Copy Permalink