//This task Runs the arm with an encoder stop at the top and a sensor stop at the bottom
void updateArm(int axisShoulder, int axisElbow, tMotor ShoulderMotor, tMotor ElbowMotor, //
tSensors touchSensor)
{
float scaleFactor = 1.0;
if(joy2Btn(8) == 1 || joy2Btn(7) == 1)
{
scaleFactor = .25;
}else{
scaleFactor = 1.00;
}
if(SensorValue(touchSensor) == 1)
{
if((axisShoulder) > 0)
{
updateMotor((int)(-axisShoulder / scaleFactor), ShoulderMotor);
}else{
motor[ShoulderMotor] = 0;
}
}else{
if(abs(nMotorEncoder[ShoulderMotor]) > ENCSTOP)
{
if((axisShoulder) < 0)
{
updateMotor((int)(-axisShoulder / scaleFactor), ShoulderMotor);
}else{
motor[ShoulderMotor] = 0;
}
}else{
updateMotor((int)(-axisShoulder / scaleFactor), ShoulderMotor);
}
}
}
// Decelerate the camera by the given amount, and update the motor speed.
// Returns TRUE if the camera is now at minimum speed, and FALSE otherwise.
bool decelerate(float* speed, float accel)
{
*speed /= accel;
// You're going too slow...
if (*speed <= MIN_SPEED) {
*speed = MIN_SPEED;
updateMotor(*speed);
return TRUE;
}
updateMotor(*speed);
return FALSE;
}
// Accelerate the camera by the given amount, and update the motor speed.
// Returns TRUE if the camera is now at maximum speed, and FALSE otherwise.
bool accelerate(float* speed, float accel)
{
*speed *= accel;
// Not so fast!
if (*speed >= MAX_SPEED) {
*speed = MAX_SPEED;
updateMotor(*speed);
return TRUE;
}
updateMotor(*speed);
return FALSE;
}
void loop()
{
//digitalWrite(LED_BUILTIN, HIGH); //so we can see if the code is running
long now = millis();
if (Serial.available() > 0) {
String inString = Serial.readStringUntil('\n');
SerialIn(now, inString);
}
//for now just simulate some data
int a1_value = random(255);
int a2_value = random(255);
if (now > (trial.trialStartMillis + trial.dur)) {
stopTrial(now);
}
if (trial.trialIsRunning==1) {
//Serial.println(String(millis()) + "," + "analogEvent" + "," + String (a1_value) + "," + String (a2_value));
}
scanImageFrame_(now);
updateMotor(now);
delay(50); //ms
//digitalWrite(LED_BUILTIN, LOW); //so we can see if the code is running
}
// ######################################################################
void ND_Navigation::updateMessage
(const RobotSimEvents::EventMessagePtr& eMsg, const Ice::Current&)
{
// LRF message
if(eMsg->ice_isA("::BeobotEvents::LRFMessage"))
{
// we got LRF data
BeobotEvents::LRFMessagePtr lrfMsg =
BeobotEvents::LRFMessagePtr::dynamicCast(eMsg);
itsDistances = lrfMsg->distances;
itsAngles = lrfMsg->angles;
LINFO("received LRF message: %d,", lrfMsg->RequestID);
// compute navigation
Beobot2::MotorCommand cmd =
itsNDNavigationAlgorithm->computeNavigationCommand
(itsDistances, itsAngles);
LINFO("received: %f %f", cmd.translation, cmd.rotation);
// send to BeoPilot
updateMotor(cmd.translation,cmd.rotation);
}
}
void update(Flywheel *flywheel)
{
float timeChange = timeUpdate(&flywheel->microTime);
measureRpm(flywheel, timeChange);
controllerUpdate(flywheel, timeChange);
updateMotor(flywheel);
// TODO: update smart motor group.
}
int MyWindow::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
{
_id = QWidget::qt_metacall(_c, _id, _a);
if (_id < 0)
return _id;
if (_c == QMetaObject::InvokeMetaMethod) {
switch (_id) {
case 0: updateMotor((*reinterpret_cast< double(*)>(_a[1])),(*reinterpret_cast< double(*)>(_a[2]))); break;
case 1: set((*reinterpret_cast< int(*)>(_a[1]))); break;
case 2: set_replay(); break;
case 3: updateCompass((*reinterpret_cast< double(*)>(_a[1]))); break;
case 4: updateScene(); break;
default: ;
}
_id -= 5;
}
return _id;
}
void jointPublish(ros::Publisher jP) {
updateMotor();
/* ************** Publish joint angles ************** */
sensor_msgs::JointState msg;// = boost::make_shared<sensor_msgs::JointState>();
// msg.reset(new sensor_msgs::JointState);
std::vector<std::string> joint_names;
std::vector<float> angles;
std::vector<float> vels;
getJointNames(joint_names);
getJointAngles(angles);
getJointVelocities(vels);
for (int i = 0; i < 2 ; ++i) {
msg.name.push_back(joint_names[i]);
msg.position.push_back(angles[i]);
msg.velocity.push_back(vels[i]);
}
msg.header.stamp = ros::Time::now();
jP.publish(msg);
}
// Entry point, containing initialization and the main draw / update loop.
int main(void)
{
int i;
// Array of all existing stars.
Star stars[STAR_COUNT];
// Records whether warp / warp effect is active.
bool warping = FALSE;
// Records whether warp is currently accelerating or decelerating.
bool accelerating = TRUE;
// The number of frames the camera has been warping at full speed for.
int warpFrames = 0;
// The current speed of the camera.
float speed = MIN_SPEED;
// Lateral camera speed.
float strafeSpeed = 0.0f;
// An interpolated version of the camera speed for the HUD bars.
float smoothSpeed = MIN_SPEED;
// Seed the RNG with a carefully constructed non-arbitrary number.
srand(0x3ae14c92);
// Prepare the LCD display and motor for use.
lcd_init();
motor_init();
// Make sure the motor is going at the initial speed.
updateMotor(speed);
// Give each star a random starting position.
for (i = 0; i < STAR_COUNT; ++i) {
randomizeStar(&stars[i]);
stars[i].z = randFloat();
}
// Main loop.
for (;;) {
// Erase all the stars from the sky. I'm assuming it's faster to do
// this than do lcd_fillScreen(BLACK).
for (i = 0; i < STAR_COUNT; ++i) {
renderStar(stars[i], speed, BLACK);
}
strafeSpeed *= 0.95f;
// Strafe left when left button is pressed.
if (input_isKeyDown(BUTTON_LEFT)) {
strafeSpeed -= 0.001f;
}
// Likewise, but for strafing right.
if (input_isKeyDown(BUTTON_RIGHT)) {
strafeSpeed += 0.001f;
}
// If the centre key has been pressed, toggle warping.
if (input_getButtonPress() == BUTTON_CENTER) {
warping = !warping;
}
// If we aren't currently warping, accept button press inputs.
if (!warping) {
// Accelerate when pressing up.
if (input_isKeyDown(BUTTON_UP)) {
accelerate(&speed, MANUAL_ACCEL);
}
// Decelerate when pressing down.
if (input_isKeyDown(BUTTON_DOWN)) {
decelerate(&speed, MANUAL_ACCEL);
}
// Otherwise, if we are in the acceleration phase of warping, speed up
// the camera until it is at maximum speed.
} else if (accelerating && accelerate(&speed, BOOST_ACCEL)) {
// When we've been at maximum speed for the given duration, start
// to decelerate.
if (++warpFrames >= BOOST_FRAMES) {
warpFrames = 0;
accelerating = FALSE;
}
// Otherwise, if we are in the deceleration phase of warping, slow down
// the camera until it is at minimum speed.
} else if (!accelerating && decelerate(&speed, BOOST_ACCEL)) {
// When we've been at minimum speed for the given duration, start
// to accelerate.
if (++warpFrames >= BOOST_FRAMES) {
warpFrames = 0;
accelerating = TRUE;
}
}
// Now loop through each star again, to update their positions and draw
// them to the display.
for (i = 0; i < STAR_COUNT; ++i) {
stars[i].z -= speed;
stars[i].x -= strafeSpeed;
// If the star is too far to the left, move it right.
if (stars[i].x < -0.5f) {
stars[i].x += 1.0f;
}
// If the star is too far to the right, move it left.
if (stars[i].x >= 0.5f) {
stars[i].x -= 1.0f;
}
// If the star is behind the camera, push it to the back of the
// scene and randomize its X and Y position.
if (stars[i].z <= 0.0f) {
stars[i].z = 1.0f;
randomizeStar(&stars[i]);
}
// Draw the star in white.
renderStar(stars[i], speed, stars[i].clr);
}
// Ease smoothSpeed towards the current value of speed.
smoothSpeed += (speed - smoothSpeed) * 0.1f;
// Draw the speed bar things on either side of the display.
renderSpeedBar(4, 4, 6, DISPLAY_HEIGHT - 8,
smoothSpeed, warping ? YELLOW : WHITE);
renderSpeedBar(DISPLAY_WIDTH - 10, 4, 6, DISPLAY_HEIGHT - 8,
smoothSpeed, warping ? YELLOW : WHITE);
// I think we have some time to spare.
wait(16);
}
// This should never happen.
return (int) (1.0 / 0.0);
}
int main(void)
{
motorInit(); // Initialize the motor port to drive the MOSFET low
uint8_t watchDogResetFlag = MCUSR & _BV(WDRF); /// Save the watchdog flag
MCUSR &= ~ _BV( WDRF ); // Clear the watchdog flag
// "In safety level 1, WDE is overridden by WDRF in MCUSR...."
// "This means that WDE is always set when WDRF is set."
// IF we left this Set, then we could get watchdogged while sleeping
wdt_enable( WDTO_8S ); // Give ourselves 8 seconds before forced reboot
enableTimer0(); // Initialize the timer that also PWMs the LEDs
WHITE_LED_DDR |= _BV(WHITE_LED_BIT); // Pin to output
RED_LED_DDR |= _BV(RED_LED_BIT);
// Button sense pin setup
BUTTON_PORT |= _BV(BUTTON_BIT); // Enable pull-up for button pin
// Battery Charger status pin setup
EOC_PORT |= _BV(EOC_BIT); // Activate pull-up
CIP_PORT |= _BV( CIP_BIT); // Activate pull-up
_delay_us(1); // Give the pull-ups a second to work
if ( !watchDogResetFlag ) { // Are we coming out of anything except for a WatchDog reset?
// Cold boot, run test mode
// Blink back and forth to show LEDs work and solicit a button press
for(uint8_t i=0;i<100 && !BUTTON_STATE_DOWN(); i++ ) {
setRedLED(255);
for(uint8_t j=0; j<100 && !BUTTON_STATE_DOWN();j++ ) {
_delay_ms(1);
}
setRedLED(0);
setWhiteLED(255);
for(uint8_t j=0; j<100 && !BUTTON_STATE_DOWN();j++ ) {
_delay_ms(1);
}
setWhiteLED(0);
wdt_reset();
}
_delay_ms(BUTTON_DEBOUNCE_TIME_MS);
// TODO: Put more code here for some testing and feedback on initial battery connection at the factory.
}
// Ready to begin normal operation!
if (BUTTON_STATE_DOWN()) { // Possible stuck button?
// If we get here, either we just finished test mode and the button is Still down, in which case
// we are testing to make sure it goes back up
// Otherwise we just reset and the button was down when we woke, so likely it is stuck down
// and that is what caused the reset. In this case, show the user and then eventually disable the button.
// Each pass of this loop takes ~1 sec.
for( uint16_t t=0; (t <= BUTTON_TRANSIT_TIMEOUT_S) && BUTTON_STATE_DOWN(); t++ ) {
// To indicate that we are in a stuck-button sequence, we will blink the white LED
// at 50% brightness, 1Hz, 10% duty cycle. This looks different than other states
// and also minimizes battery usage (the LED pulls 10+mA) since we might be doing this
// for many minutes
// Start with LED off because it looks better when we are coming in from a watchdog
// reset because the button was held down for more than 8 secs. Otherwise user
// sees an odd blink pattern.
setWhiteLED(0);
// Leave the white LED off for 900 ms or until the button goes up
for( uint8_t l=0; l<90 && BUTTON_STATE_DOWN() ; l++) {
_delay_ms(10);
}
setWhiteLED(BUTTON_FEEDBACK_BRIGHTNESS);
// Leave the white LED on for 100 ms or until the button goes up
// Could do this as a single _delay_ms(100) but that might feel un-responsive
for( uint8_t l=0; l<10 && BUTTON_STATE_DOWN() ; l++) {
_delay_ms(10);
}
wdt_reset();
}
// Turn off LED before continuing
setWhiteLED(0);
// Debounce the possible button up transition
_delay_ms(BUTTON_DEBOUNCE_TIME_MS);
}
if (BUTTON_STATE_DOWN()) { // Do we still have a stuck button?
// Indicate we are entering transit mode with a quick double flash of both LEDs
setRedLED(255);
setWhiteLED(255);
_delay_ms(100);
setRedLED(0);
setWhiteLED(0);
_delay_ms(100);
setRedLED(255);
setWhiteLED(255);
_delay_ms(100);
setRedLED(0);
setWhiteLED(0);
BUTTON_PORT &= ~_BV(BUTTON_BIT); // Disable pull up to avoid running the battery down.
// Do not enable interrupt on button pin change - we will require a charger state change to wake up
// Since the interrupt is not enabled, the pin will be disconnected during sleep so any floating
// on it will not waste power.
} else {
// Leave pull-up enabled
PCMSK1 = _BV(BUTTON_INT); // Enable interrupt on button pin so we wake on a press
}
PCMSK0 = _BV(EOC_INT) | _BV(CIP_INT); // Enable interrupt on change in state-of-charge pin or end-of-charge pin no matter what
GIMSK |= _BV(PCIE1) | _BV(PCIE0); // Enable both pin change interrupt vectors (each individual pin was also be enabled above)
// Clear pending interrupt flags. This way we will only get an interrupt if something changes
// after we read it. There is a race condition where something could change between the flag clear and the
// reads below, so code should be able to deal with possible redundant interrupt and worst case
// is that we get woken up an extra time and go back to sleep.
GIFR = _BV(PCIF1) | _BV(PCIF0); // Clear interrupt flags so we will interrupt on any change after now...
if ( !CIP_STATE_ACTIVE() && !EOC_STATE_ACTIVE() ) { // Check if conditions are ALREADY true since we only wake on change....
// Ok, it is bedtime!
set_sleep_mode( SLEEP_MODE_PWR_DOWN ); // Go into deep sleep where only a pin change can wake us.. uses only ~0.1uA!
// GOOD NIGHT!
// This code disables the Watchdog. Note that we can not use the library wdt_disable() becuase it has a bug
// that causes intermittent unwanted resets.
// Note interrupts are already clear when we get here, otherwise we would need to worry about getting interrupted between the two following lines
WDTCSR |= _BV(WDCE) | _BV(WDE); // In the same operation, write a logic one to WDCE and WDE.
// Note we use OR to preserve the prescaler
WDTCSR = 0; // Within the next four clock cycles, in the same operation, write the WDE and WDP bits
// as desired, but with the WDCE bit cleared.
sleep_enable(); // "To enter any of the three sleep modes, the SE bit in MCUCR must be written to logic one and a SLEEP instruction must be executed."
sei(); // Enable global interrupts. "When using the SEI instruction to enable interrupts, the instruction following SEI will be executed before any pending interrupts."
sleep_cpu(); // This must come right after the sei() to avoid race condition
// GOOD MORNING!
// If we get here, then a button push or change in charger status woke s up....
sleep_disable(); // "To avoid the MCU entering the sleep mode unless it is the programmer’s purpose, it is recommended to write the Sleep Enable (SE) bit to one just before the execution of the SLEEP instruction and to clear it immediately after waking up."
cli(); // We are awake now, and do don't care about interrupts anymore (out interrupt routines don't do anything anyway)
wdt_enable( WDTO_8S ); // Re-enable watchdog on wake Give ourselves 8 seconds before reboot
}
// Ok, now we are running!!!
// Motor speed
uint8_t currentSpeedStep = 0; // What motor speed setting are we currently on?
while (1) {
// This main loop runs for as long as the motor is on.
// It can be terminated by battery charger change of state, low battery detection, button press back to 0 speed, or long button press
// All these changes terminate the loop in a reboot.
if (EOC_STATE_ACTIVE()) { // End of charge?
motorOff(); //Turn motor off in case were running before plug went in
setWhiteLED(255); // White LED full on
_delay_ms( JACK_DEBOUNCE_TIME_MS );
while (EOC_STATE_ACTIVE()); // White LED on for as long as we are charging....
// Note that this will watchdog timeout after 8 seconds and reboot us,
// After which we will immediately fall right back to here and continue to show the white LED
setWhiteLED(0); // Turn it off now, for instant feedback if unplugged (otherwise it will be on for extra 250ms waiting for watchdog reset)
// Charger unplugged, reboot for goo measure
REBOOT();
}
if (CIP_STATE_ACTIVE()) { // Charging?
motorOff(); //Turn motor off in case were running before plug went in
uint8_t brightness=0;
int8_t direction=1;
_delay_ms( JACK_DEBOUNCE_TIME_MS );
while (CIP_STATE_ACTIVE()) { // White LED pulse for as long as we are charging....
setWhiteLED(brightness);
if (brightness==255) {
direction=-1;
} else if (brightness==0) {
direction=1;
}
brightness+=direction;
_delay_ms(1); // Slows the speed of the rampping LED
wdt_reset();
}
setWhiteLED(0); // Turn it off now, for instant feedback if unplugged (otherwise it will be on for extra 250ms waiting for watchdog reset)
// All done charing, reboot for good measure
REBOOT();
}
uint8_t vccx10 = readVccVoltage(); // Capture the current power supply voltage. This takes ~1ms and will be needed multiple times below
if ( vccx10 <= LOW_BATTERY_VOLTS_COLDx10) {
if ( (currentSpeedStep==0) || ( vccx10 <= LOW_BATTERY_VOLTS_WARMx10) ) { // Motor off, or running and really low?
motorOff();
setWhiteLED(0); // Needed becuase both LEDs might be on if we are in the middle of a button press
setRedLED(255);
_delay_ms(LOW_BATTERY_LED_ONTIME_MS); // Show red LED to user to show low battery
while (BUTTON_STATE_DOWN()); // Wait for button to be released if pressed
// Will watchdog timeout in 8 seconds if stuff
setRedLED(0);
REBOOT();
}
}
uint8_t buttonPressedFlag=0;
if (BUTTON_STATE_DOWN()) { // Button pushed?
setWhiteLED(BUTTON_FEEDBACK_BRIGHTNESS);
_delay_ms(BUTTON_DEBOUNCE_TIME_MS); // debounce going down...
if ( currentSpeedStep ==0 ) { // Special case first press turning on instantly
updateMotor( pgm_read_word(&speedSteps[1].top) , pgm_read_word(&speedSteps[1].normailzedDuty), vccx10); // Set new motor speed
}
uint16_t buttonDownCount=0;
while (BUTTON_STATE_DOWN()) { // Wait for button to go back up or longpress timeout
if (buttonDownCount++ >= BUTTON_LONG_PRESS_MS ) { // Long press? Shut motor off
// The reboot would do both of these anyway, but we do them redundantly here so UI feels responsive-
// The full reboot cycle takes 100+ ms.
motorOff();
setWhiteLED(0);
REBOOT();
// If the button is still down once we reboot, we will land in the stuck button detection sequence
// which will blink the LED for 1/10th second every second until either the button goes up
// or the transit mode button timeout expires
}
_delay_ms(1); // One loop=~1ms
}
// Pressed less than a long press
buttonPressedFlag=1; // Debounce after setting new motor speed so UI feels responsive
currentSpeedStep++;
if (currentSpeedStep >= SPEED_STEP_COUNT ) { // Cycled all the Way around?
currentSpeedStep=0;
}
}
updateMotor( pgm_read_word(&speedSteps[currentSpeedStep].top) , pgm_read_word(&speedSteps[currentSpeedStep].normailzedDuty), vccx10); // Set new motor speed
if (buttonPressedFlag) {
// Button released, white LED off again
setWhiteLED(0);
_delay_ms(BUTTON_DEBOUNCE_TIME_MS); // debounce the button returning back up
}
if (currentSpeedStep==0) { // Either we stepped though the settings back to off, or we got a spurious wake up
REBOOT();
}
// If we get to here, then we check for a low battery and had the chance to reboot if we found one,
// so ok to postpone reset...
wdt_reset();
}
}
void motorOn() {
MotorEnable = true;
updateMotor();
}
void setSpeed(const int16_t speedL, const int16_t speedR) {
OutputLeft = speedL;
OutputRight = speedR;
updateMotor();
}
//Set the target speed using millimeter metrics
void setSpeedMM(const float speedMML, const float speedMMR) {
OutputLeft = (int16_t) (speedMML * PulseperMM * scPeriod * 5 / 1000);
OutputRight = (int16_t) (speedMMR * PulseperMM * scPeriod * 5 / 1000);
updateMotor();
}
