#include #include #include #include #include #include #include #include #include #include #include #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);