149 lines
4.6 KiB
C++
149 lines
4.6 KiB
C++
#include "main.h"
|
|
#include "pros/serial.hpp"
|
|
#include <bitset>
|
|
|
|
#include <sstream>
|
|
|
|
// extern "C" int32_t vexGenericSerialReceive( uint32_t index, uint8_t *buffer, int32_t length );
|
|
// extern "C" void vexGenericSerialTransmit( uint32_t index, uint8_t *buffer, int32_t length );
|
|
// extern "C" void vexGenericSerialWriteChar( uint32_t index, uint8_t chara );
|
|
// extern "C" void vexGenericSerialFlush( uint32_t index);
|
|
// extern "C" void vexGenericSerialEnable( uint32_t index, uint32_t nu );
|
|
// extern "C" void vexGenericSerialBaudrate( uint32_t index, uint32_t rate );
|
|
|
|
/**
|
|
* A callback function for LLEMU's center button.
|
|
*
|
|
* When this callback is fired, it will toggle line 2 of the LCD text between
|
|
* "I was pressed!" and nothing.
|
|
*/
|
|
|
|
#define SERIALPORT 11
|
|
// Variable to put the gyro value into
|
|
double gyroValue = 0;
|
|
|
|
// code bleow here works! for recv
|
|
// void serialRead(void* params) {
|
|
|
|
// // Start serial on desired port
|
|
// vexGenericSerialEnable( SERIALPORT - 1, 0 );
|
|
|
|
// // Set BAUD rate
|
|
// vexGenericSerialBaudrate( SERIALPORT - 1, 9600 );
|
|
|
|
// // Let VEX OS configure port
|
|
// pros::delay(10);
|
|
|
|
// // Serial message format:
|
|
// // D[LIDAR DIST]I[IR DATA]A[GYRO ANGLE]E
|
|
// // Example Message:
|
|
// // D50.2I128A12.32E
|
|
|
|
// while (true) {
|
|
|
|
// // Buffer to store serial data
|
|
// uint8_t buffer[256];
|
|
// int len = 256;
|
|
// uint8_t data[3];
|
|
// data[0] = 2;
|
|
// data[1] = 25;
|
|
// data[2] = 44;
|
|
// // Get serial data
|
|
// //vexGenericSerialTransmit(SERIALPORT - 1, data, 3);
|
|
// vexGenericSerialWriteChar(SERIALPORT - 1, data[2]);
|
|
// vexGenericSerialFlush(SERIALPORT - 1);
|
|
// pros::delay(50);
|
|
// int32_t nRead = vexGenericSerialReceive(SERIALPORT - 1, buffer, len);
|
|
// pros::lcd::print(3,"%d", nRead);
|
|
|
|
// // Delay to let serial data arrive
|
|
// pros::delay(50);
|
|
// }
|
|
// }
|
|
|
|
void on_center_button() {
|
|
static bool pressed = false;
|
|
pressed = !pressed;
|
|
if (pressed) {
|
|
pros::lcd::set_text(2, "I was pressed!");
|
|
} else {
|
|
pros::lcd::clear_line(2);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Runs initialization code. This occurs as soon as the program is started.
|
|
*
|
|
* All other competition modes are blocked by initialize; it is recommended
|
|
* to keep execution time for this mode under a few seconds.
|
|
*/
|
|
void initialize() {
|
|
pros::lcd::initialize();
|
|
pros::lcd::set_text(1, "Hello PROS User!");
|
|
|
|
pros::lcd::register_btn1_cb(on_center_button);
|
|
}
|
|
|
|
/**
|
|
* Runs while the robot is in the disabled state of Field Management System or
|
|
* the VEX Competition Switch, following either autonomous or opcontrol. When
|
|
* the robot is enabled, this task will exit.
|
|
*/
|
|
void disabled() {}
|
|
|
|
/**
|
|
* Runs after initialize(), and before autonomous when connected to the Field
|
|
* Management System or the VEX Competition Switch. This is intended for
|
|
* competition-specific initialization routines, such as an autonomous selector
|
|
* on the LCD.
|
|
*
|
|
* This task will exit when the robot is enabled and autonomous or opcontrol
|
|
* starts.
|
|
*/
|
|
void competition_initialize() {}
|
|
|
|
/**
|
|
* Runs the user autonomous code. This function will be started in its own task
|
|
* with the default priority and stack size whenever the robot is enabled via
|
|
* the Field Management System or the VEX Competition Switch in the autonomous
|
|
* mode. Alternatively, this function may be called in initialize or opcontrol
|
|
* for non-competition testing purposes.
|
|
*
|
|
* If the robot is disabled or communications is lost, the autonomous task
|
|
* will be stopped. Re-enabling the robot will restart the task, not re-start it
|
|
* from where it left off.
|
|
*/
|
|
void autonomous() {}
|
|
|
|
/**
|
|
* Runs the operator control code. This function will be started in its own task
|
|
* with the default priority and stack size whenever the robot is enabled via
|
|
* the Field Management System or the VEX Competition Switch in the operator
|
|
* control mode.
|
|
*
|
|
* If no competition control is connected, this function will run immediately
|
|
* following initialize().
|
|
*
|
|
* If the robot is disabled or communications is lost, the
|
|
* operator control task will be stopped. Re-enabling the robot will restart the
|
|
* task, not resume it from where it left off.
|
|
*/
|
|
void opcontrol() {
|
|
pros::Controller master(pros::E_CONTROLLER_MASTER);
|
|
pros::Motor left_mtr(1);
|
|
pros::Motor right_mtr(2);
|
|
// Start serial task
|
|
//pros::Task gyroTask (serialRead);
|
|
pros::Serial mySerial(SERIALPORT, 9600);
|
|
mySerial.flush();
|
|
while (true) {
|
|
//pros::lcd::print(4,"%f", gyroValue);
|
|
while (mySerial.get_read_avail() == 0) {
|
|
pros::delay(1);
|
|
}
|
|
pros::lcd::print(3, "%x", std::bitset<32>(mySerial.read_byte()));
|
|
mySerial.write_byte(44);
|
|
pros::delay(20);
|
|
}
|
|
}
|