mitchellhansen/battletank
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

sorta working, control needs to be improved

Browse Source
This commit is contained in:
mitchellhansen
2020-10-01 14:04:35 -07:00
parent f86e4d7c52
commit 90b9831079
62 changed files with 3221 additions and 510 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
remote_main.cpp
View File

@@ -1,5 +1,5 @@
#include <util/delay.h>
#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>
#include <stdarg.h>
#include <util/setbaud.h>
@@ -11,10 +11,8 @@
#include <nRF24L01.h>
#include <RF24.h>
#define CE_PIN 3
#define CS_PIN 2
#define RED 4
#define GREEN 5
#define CE_PIN 7
#define CS_PIN 8
class Radio {
@@ -32,44 +30,126 @@ public:
const uint64_t dozer_pipe = 0xF0F0F0F0D2LL;
const uint64_t remote_pipe = 0xF0F0F0F0E1LL;
#pragma pack(8)
struct StickValues {
uint32_t stick_1_a;
uint32_t stick_1_b;
uint32_t stick_2_a;
uint32_t stick_2_b;
int16_t stick_1_y;
int16_t stick_1_x;
int16_t stick_2_y;
int16_t stick_2_x;
};
#define PI 3.141592654
#define DEG(rad) (rad*180.0/PI)
int main() {
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
uart_init();
init();
initVariant();
SPI.begin();
Serial.begin(9600);
RF24 radio(CE_PIN, CS_PIN);
radio.begin();
radio.setChannel(100);
radio.setPALevel (RF24_PA_MAX);
radio.setDataRate(RF24_1MBPS);
radio.openWritingPipe(remote_pipe);
radio.openReadingPipe(1, dozer_pipe);
radio.startListening();
bool breaking = false;
while (!breaking) {
delay(1000);
_delay_ms(100);
int16_t ref_stick_1_y = analogRead(A4); // RY
int16_t ref_stick_1_x = analogRead(A5); // RX
int16_t ref_stick_2_y = analogRead(A6); // LY
int16_t ref_stick_2_x = analogRead(A7); // LX
// Get input
while (true) {
// Parse into a struct
StickValues stick_values;
stick_values.stick_1_a = 100;
stick_values.stick_1_b = 50;
stick_values.stick_2_a = 0;
stick_values.stick_2_b = 0;
stick_values.stick_1_y = analogRead(A5); // RY
stick_values.stick_1_x = analogRead(A4); // RX
stick_values.stick_2_y = analogRead(A6); // LY
stick_values.stick_2_x = analogRead(A7); // LX
int16_t delta_1_y = stick_values.stick_1_y - ref_stick_1_y;
int16_t delta_1_x = ref_stick_1_x - stick_values.stick_1_x;
int16_t delta_2_y = stick_values.stick_2_y - ref_stick_2_y;
int16_t delta_2_x = stick_values.stick_2_x - ref_stick_2_x;
stick_values.stick_1_y = delta_1_y / 2;
stick_values.stick_1_x = delta_1_x / 2;
stick_values.stick_2_y = delta_2_y / 2;
stick_values.stick_2_x = delta_2_x / 2;
// Send to dozer
radio.stopListening();
radio.write(&stick_values, sizeof(uint32_t)*4);
delay(10);
if (radio.write(&stick_values, sizeof(int16_t)*4))
{
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);
}
//Serial.println(mag);
Serial.print(ang);
Serial.print(" : ");
Serial.print(mag);
Serial.print(" : [");
Serial.print(left_track);
Serial.print(", ");
Serial.print(right_track);
Serial.println("]");
//Serial.println("");
}
radio.startListening();
if (serialEventRun) serialEventRun();
}
}