diff --git a/EcoBot.ino b/EcoBot.ino index d368db0..5d5be30 100644 --- a/EcoBot.ino +++ b/EcoBot.ino @@ -14,7 +14,7 @@ #define DELAY_1_SECOND 1000 #define DELAY_DEFAULT DELAY_1_SECOND #define SERIAL_BRATE 115200 -static byte devStuff = E_OK; +static byte devStuff = E_NOT_OK; /* General Stuff end */ /* Motor Stuff */ @@ -29,30 +29,41 @@ static byte devStuff = E_OK; #define DRV8834_MOTOR_BOTH 3u #define DRV8834_DIRECTION_BACKWARD 0u #define DRV8834_DIRECTION_FORWARD 1u -#define DRV8834_POWER_FULL 255u -#define DRV8834_POWER_HALF 127u +#define DRV8834_POWER_FULL 255u /* Doesn't work with less than max, lower than 3V is not enough for motors or PWM is sketchy */ #define DRV8834_POWER_NONE 0u -#define DRV8834_WAKEUP_WAIT 1 /* Miliseconds until DRV8834 should be fully working after wakeup */ +#define DRV8834_WAKEUP_WAIT 1 /* Miliseconds until DRV8834 should be fully working after wakeup */ #define DRV8834_WALK_TIME 100 -#define DRV8834_BREAK_TIMEOUT 115 /* Lower than this and the timeout is too short */ +#define DRV8834_BREAK_TIMEOUT 115 /* Lower than this and the timeout is too short */ +#define MOTOR_TURN_TIME_FACTOR 4 /* Multiply by degree to turn around. Shoulkd be time for 1 degree */ /* Motor Stuff end */ /* Exploration Stuff */ -#define EXPLORE_AUTOMATE 0u /* Autonomous driving */ -#define EXPLORE_MANUAL 1u /* Manual driving from IR */ - -static byte exploreState = EXPLORE_AUTOMATE; +#define EXPLORE_AUTOMATE 0u /* Autonomous driving */ +#define EXPLORE_MANUAL 1u /* Manual driving from IR */ +#define EXPLORE_ROTATE_45 45 +#define EXPLORE_ROTATE_90 90 +#define EXPLORE_ROTATE_180 180 +#define EXPLORE_ROTATE_CUSTOM 135 +#define EXPLORE_ROTATE_DEFAULT EXPLORE_ROTATE_90 +#define EXPLORE_OBSTACLE_NONE 0u +#define EXPLORE_OBSTACLE_LEFT 1u +#define EXPLORE_OBSTACLE_RIGHT 2u +#define EXPLORE_OBSTACLE_BOTH 3u + +static byte exploreState = EXPLORE_MANUAL; /* Exploration Stuff end */ /* IR Stuff */ -#define PIN_IR_RECEIVER_POWER A0 /* Power the IR Receiver with this pin */ -#define PIN_IR_RECEIVER_DATA 9 /* Read data from IR Receiver with this pin */ +#define PIN_IR_RECEIVER_POWER A0 /* Power the IR Receiver with this pin */ +#define PIN_IR_OBSTACLE_DATA 8 /* Read data from IR Obstacle Detector with this pin */ +#define PIN_IR_RECEIVER_DATA 9 /* Read data from IR Receiver with this pin */ #define IR_VALUE_FORWARD 0xFF18E7 /* Move Backward */ #define IR_VALUE_BACKWARD 0xFF4AB5 /* Move Forward */ #define IR_VALUE_LEFT 0xFF10EF /* Rotate Left */ #define IR_VALUE_RIGHT 0xFF5AA5 /* Rotate Right */ #define IR_VALUE_MODE 0xFF38C7 /* Switch between Manual and Automate Exploring */ +#define IR_VALUE_SLEEP 0xFF6897 /* Go to Sleep for a while */ IRrecv irrecv(PIN_IR_RECEIVER_DATA); decode_results results; @@ -66,7 +77,10 @@ decode_results results; #define BATTERY_SLEEP_THRESHOLD 3.3 /* Voltage drops by 0.05 V when motors are working */ #define ROBOT_SLEEP_1_SECOND 1 #define ROBOT_SLEEP_10_SECONDS 10 +#define ROBOT_SLEEP_30_SECONDS 30 #define ROBOT_SLEEP_1_MINUTE 60 +#define ROBOT_SLEEP_10_MINUTES 600 +#define ROBOT_SLEEP_30_MINUTES 1800 #define ROBOT_SLEEP_TIME_DEFAULT ROBOT_SLEEP_1_SECOND /* Power Management Stuff end */ @@ -161,7 +175,7 @@ void Motor_SwitchDirection(byte motorIdentifier, byte motorDirection) /*************************************************************************************** * Function: Motor_EnableMotor() *************************************************************************************** - * Description: This function will switch the direction of a specified motor + * Description: This function will power a specified motor * Parameters: * - motorIdentifier[in] : Identifier of the motor to be switched forward * Supported Inputs: @@ -199,6 +213,58 @@ void Motor_EnableMotor(byte motorIdentifier, byte motorPower) } } +/*************************************************************************************** + * Function: Motor_RotateRight() + *************************************************************************************** + * Description: This function will use motors to rotate right. + * Parameters: + * - degrees[in] : Number of degrees to rotate right + **************************************************************************************/ +void Motor_RotateRight(uint16_t degrees) +{ + /* Stop Walking */ + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + + /* Rotate around */ + Motor_SwitchDirection(DRV8834_MOTOR_A, DRV8834_DIRECTION_FORWARD); + Motor_SwitchDirection(DRV8834_MOTOR_B, DRV8834_DIRECTION_BACKWARD); + + /* Try to rotate slowly */ + do + { + Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL); + delay(MOTOR_TURN_TIME_FACTOR); + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + degrees--; + }while(0 < degrees); +} + +/*************************************************************************************** + * Function: Motor_RotateLeft() + *************************************************************************************** + * Description: This function will use motors to rotate left. + * Parameters: + * - degrees[in] : Number of degrees to rotate left + **************************************************************************************/ +void Motor_RotateLeft(uint16_t degrees) +{ + /* Stop Walking */ + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + + /* Rotate around */ + Motor_SwitchDirection(DRV8834_MOTOR_A, DRV8834_DIRECTION_BACKWARD); + Motor_SwitchDirection(DRV8834_MOTOR_B, DRV8834_DIRECTION_FORWARD); + + /* Try to rotate slowly */ + do + { + Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL); + delay(MOTOR_TURN_TIME_FACTOR); + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + degrees--; + }while(0 < degrees); +} + /*************************************************************************************** * Function: Motor_TestMotor() *************************************************************************************** @@ -216,21 +282,134 @@ void Motor_TestMotor(byte motorIdentifier) /* To test the motor functionality, the motor shall run in each direction * and also the switching of the direction shall work. */ - static byte direction = 1u; + static byte direction = DRV8834_DIRECTION_FORWARD; /* Run the motor for 1s */ - Motor_EnableMotor(motorIdentifier, 255u); + Motor_EnableMotor(motorIdentifier, DRV8834_POWER_FULL); digitalWrite(LED_BUILTIN, HIGH); delay(DELAY_1_SECOND); /* Stop the motor for 1s */ - Motor_EnableMotor(motorIdentifier, 0u); + Motor_EnableMotor(motorIdentifier, DRV8834_POWER_NONE); digitalWrite(LED_BUILTIN, LOW); delay(DELAY_1_SECOND); /* Switch Motor Direction */ - Motor_SwitchDirection(motorIdentifier, !direction); direction = !direction; + Motor_SwitchDirection(motorIdentifier, direction); +} + +/*************************************************************************************** + * Function: Robot_WakeUp() + *************************************************************************************** + * Description: This function is executed when the robot wakes up. It will initialize + * everything and will make the robot functional. + **************************************************************************************/ +void Robot_WakeUp(void) +{ + /* Initialize things */ + /* PIN Modes */ + pinMode(LED_BUILTIN, OUTPUT); /* Debug purposes */ + pinMode(PIN_MA_ENABLE, OUTPUT); + pinMode(PIN_MA_PHASE, OUTPUT); + pinMode(PIN_MB_ENABLE, OUTPUT); + pinMode(PIN_MB_PHASE, OUTPUT); + pinMode(PIN_DRV8834_SLEEP, OUTPUT); + pinMode(PIN_IR_RECEIVER_POWER, OUTPUT); + pinMode(PIN_IR_RECEIVER_DATA, INPUT); + + /* Wakeup the Motor Driver */ + digitalWrite(PIN_DRV8834_SLEEP, HIGH); + delay(DRV8834_WAKEUP_WAIT); + + /* Set default direction of both Motors to move Forward */ + Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_FORWARD); + + /* Power on the IR Receiver */ + digitalWrite(PIN_IR_RECEIVER_POWER, HIGH); + + /* Enable IR Receiver */ + irrecv.enableIRIn(); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + /* New Day! */ + Serial.println("Good Morning!"); + } +} + +/*************************************************************************************** + * Function: Robot_Sleep() + *************************************************************************************** + * Description: This function tries to make the Robot Sleep. + * Parameters: + * - sleepTime[in] : Number of seconds to go to sleep + * Supported Inputs: + * 0u - 3600u + **************************************************************************************/ +void Robot_Sleep(uint16_t sleepTime) +{ + /* Check if Robot is able to sleep */ + if(HIGH == digitalRead(PIN_INSOMNIA)) + { + /* Insomnia is here, can't sleep */ + } + else + { + /* Insomnia is not around, sleep */ + /* Set wakeup conditions */ + /* GPIO External Interrupt Wakeup */ + attachInterrupt(PIN_INSOMNIA, Robot_WakeUp, HIGH); + + /* Ensure All Used Pins are configured to output, then output nothing */ + /* Exception for Insomnia to wake it up and Battery Level as it will be always on */ + pinMode(PIN_IR_RECEIVER_POWER, OUTPUT); + digitalWrite(PIN_IR_RECEIVER_POWER, LOW); /* Disable IR Receiver */ + pinMode(PIN_IR_RECEIVER_DATA, OUTPUT); + digitalWrite(PIN_IR_RECEIVER_DATA, LOW); + pinMode(PIN_DRV8834_SLEEP, OUTPUT); + digitalWrite(PIN_DRV8834_SLEEP, LOW); /* Send Motor Driver to sleep */ + pinMode(PIN_MA_ENABLE, OUTPUT); + digitalWrite(PIN_MA_ENABLE, LOW); + pinMode(PIN_MA_PHASE, OUTPUT); + digitalWrite(PIN_MA_PHASE, LOW); + pinMode(PIN_MB_ENABLE, OUTPUT); + digitalWrite(PIN_MB_ENABLE, LOW); + pinMode(PIN_MB_PHASE, OUTPUT); + digitalWrite(PIN_MB_PHASE, LOW); + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, LOW); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + /* Say Good night */ + Serial.println("Good night!"); + + delay(100); + } + + /* Go to sleep */ + if(sleepTime & 1u) + { + LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF); + } + if(sleepTime & 2u) + { + LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF); + } + if(sleepTime & 4u) + { + LowPower.powerDown(SLEEP_4S, ADC_OFF, BOD_OFF); + } + + while(sleepTime & 0xFFF8u) + { + sleepTime -= 8; + LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF); + } + } } /*************************************************************************************** @@ -240,6 +419,12 @@ void Motor_TestMotor(byte motorIdentifier) **************************************************************************************/ void HandleIR(void) { + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.print("IR: "); + Serial.println(results.value, HEX); + } /* Robot reaction based on the IR value */ switch(results.value) { @@ -270,12 +455,206 @@ void HandleIR(void) exploreState = !exploreState; Motor_BreakMotor(DRV8834_MOTOR_BOTH); break; + case IR_VALUE_SLEEP: + /* Sleep for a while */ + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + Robot_Sleep(ROBOT_SLEEP_1_MINUTE); + Robot_WakeUp(); + break; default: /* Do nothing */ break; } } +/*************************************************************************************** + * Function: Robot_DetectAhead() + *************************************************************************************** + * Description: Read sensors so we know if the road is blocked. + **************************************************************************************/ +byte Robot_DetectAhead(void) +{ + /* Return if there is any obstacle ahead */ + if(LOW == digitalRead(PIN_IR_OBSTACLE_DATA)) + { + /* Obstacle ahead */ + return E_NOT_OK; + } + else + { + /* No obstacle ahead */ + } + + return E_OK; +} + +/*************************************************************************************** + * Function: Robot_LookAround() + *************************************************************************************** + * Description: Return a value which correspond to obstacles ahead. + * return EXPLORE_OBSTACLE_NONE == 0 : Left and Right are both free + * EXPLORE_OBSTACLE_LEFT == 1 : Left is blocked + * EXPLORE_OBSTACLE_RIGHT == 2 : Right is blocked + * EXPLORE_OBSTACLE_BOTH == 3 : Both Left and Right are blocked + **************************************************************************************/ +byte Robot_LookAround(void) +{ + /* Return Left and Right Readings */ + byte retVal = EXPLORE_OBSTACLE_NONE; + + /* Look Left */ + Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT); + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + delay(DELAY_DEFAULT); + if(E_NOT_OK == Robot_DetectAhead()) + { + /* Remove this way */ + retVal += EXPLORE_OBSTACLE_LEFT; + } + + /* Center back */ + Motor_RotateRight(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + + /* Look Right */ + Motor_RotateRight(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + if(E_NOT_OK == Robot_DetectAhead()) + { + /* Remove this way as well */ + retVal += EXPLORE_OBSTACLE_RIGHT; + } + + /* Return to original position */ + Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.print("Around: "); + Serial.println(retVal, HEX); + } + + return retVal; +} + +/*************************************************************************************** + * Function: Robot_Autopilot() + *************************************************************************************** + * Description: This function will use Robot intelligence for driving around. + **************************************************************************************/ +void Robot_Autopilot(void) +{ + /* Each bit represents one way: Left, ahead(or back) and Right <-- MSB to LSM */ + byte whereToGo = 2u; + uint16_t randomDegrees = 180; + + /* Read Distance Sensors and decide if movement is possible */ + /* - IR Sensor detects on LOW read */ + if(E_NOT_OK == Robot_DetectAhead()) + { + /* Obstacle Detected Ahead */ + /* Stop */ + Motor_BreakMotor(DRV8834_MOTOR_BOTH); + delay(DELAY_DEFAULT); + + /* Some Dev Stuff */ + if(E_OK == devStuff) + { + Serial.println("I see"); + } + + /* Look around */ + whereToGo = Robot_LookAround(); + + /* Decide which way to go */ + /* Ahead is already blocked */ + switch(whereToGo) + { + case EXPLORE_OBSTACLE_NONE: + /* Only obstacle ahead */ + /* Choose Left or Right at random */ + randomSeed(millis()); + if(E_OK == random(2) % 2) + { + /* Go Right */ + Motor_RotateRight(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.println("3 -> Go Right"); + } + } + else + { + /* Go Left */ + Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.println("3 -> Go Left"); + } + } + + break; + case EXPLORE_OBSTACLE_BOTH: + /* Only way is to turn around */ + /* Turn around random degrees */ + randomSeed(millis()); + randomDegrees = (uint16_t)random(360); + Motor_RotateLeft(randomDegrees); + delay(DELAY_DEFAULT); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.println("0 -> Go Random"); + } + + break; + case EXPLORE_OBSTACLE_RIGHT: + /* Obstacle in Right side */ + Motor_RotateLeft(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.println("1 -> Go Left"); + } + + break; + case EXPLORE_OBSTACLE_LEFT: + /* Obstacle in Left side */ + Motor_RotateRight(EXPLORE_ROTATE_DEFAULT); + delay(DELAY_DEFAULT); + + /* Dev Stuff */ + if(E_OK == devStuff) + { + Serial.println("2 -> Go Right"); + } + + break; + default: + /* If i panic i do nothing */ + break; + } + } + else + { + /* No obstacle ahead */ + /* Walk forward */ + Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_FORWARD); + Motor_EnableMotor(DRV8834_MOTOR_BOTH, DRV8834_POWER_FULL); + } +} + /*************************************************************************************** * Function: Robot_Explore() *************************************************************************************** @@ -293,7 +672,7 @@ void Robot_Explore(void) static volatile long breakTime = 0; - /* Check Exploreing state */ + /* Check Exploring state */ if(exploreState == EXPLORE_AUTOMATE) { /* Try to read IR Receiver */ @@ -321,9 +700,7 @@ void Robot_Explore(void) } /* Do Autonomous things */ - /* Check for Obstacles */ - /* Decide next Direction */ - /* Move */ + Robot_Autopilot(); } else { @@ -359,125 +736,12 @@ void Robot_Explore(void) } } -/*************************************************************************************** - * Function: Robot_WakeUp() - *************************************************************************************** - * Description: This function is executed when the robot wakes up. It will initialize - * everything and will make the robot functional. - **************************************************************************************/ -void Robot_WakeUp(void) -{ - /* Initialize things */ - /* PIN Modes */ - pinMode(LED_BUILTIN, OUTPUT); /* Debug purposes */ - pinMode(PIN_MA_ENABLE, OUTPUT); - pinMode(PIN_MA_PHASE, OUTPUT); - pinMode(PIN_MB_ENABLE, OUTPUT); - pinMode(PIN_MB_PHASE, OUTPUT); - pinMode(PIN_DRV8834_SLEEP, OUTPUT); - pinMode(PIN_IR_RECEIVER_POWER, OUTPUT); - pinMode(PIN_IR_RECEIVER_DATA, INPUT); - - /* Wakeup the Motor Driver */ - digitalWrite(PIN_DRV8834_SLEEP, HIGH); - delay(DRV8834_WAKEUP_WAIT); - - /* Set default direction of both Motors to move Forward */ - Motor_SwitchDirection(DRV8834_MOTOR_BOTH, DRV8834_DIRECTION_FORWARD); - - /* Power on the IR Receiver */ - digitalWrite(PIN_IR_RECEIVER_POWER, HIGH); - - /* Enable IR Receiver */ - irrecv.enableIRIn(); - - /* Dev Stuff */ - if(E_OK == devStuff) - { - /* New Day! */ - Serial.println("Good Morning!"); - } -} - -/*************************************************************************************** - * Function: Robot_Sleep() - *************************************************************************************** - * Description: This function checks the Energy Consumption and decide what to do. - * Parameters: - * - sleepTime[in] : Number of seconds to go to sleep - * Supported Inputs: - * 0u - 3600u - **************************************************************************************/ -void Robot_Sleep(uint16_t sleepTime) -{ - /* Check if Robot is able to sleep */ - if(HIGH == digitalRead(PIN_INSOMNIA)) - { - /* Insomnia is here, can't sleep */ - } - else - { - /* Insomnia is not around, sleep */ - /* Set wakeup conditions */ - /* GPIO External Interrupt Wakeup */ - attachInterrupt(PIN_INSOMNIA, Robot_WakeUp, HIGH); - - /* Ensure All Used Pins are configured to output, then output nothing */ - /* Exception for Insomnia to wake it up and Battery Level as it will be always on */ - pinMode(PIN_IR_RECEIVER_POWER, OUTPUT); - digitalWrite(PIN_IR_RECEIVER_POWER, LOW); /* Disable IR Receiver */ - pinMode(PIN_IR_RECEIVER_DATA, OUTPUT); - digitalWrite(PIN_IR_RECEIVER_DATA, LOW); - pinMode(PIN_DRV8834_SLEEP, OUTPUT); - digitalWrite(PIN_DRV8834_SLEEP, LOW); /* Send Motor Driver to sleep */ - pinMode(PIN_MA_ENABLE, OUTPUT); - digitalWrite(PIN_MA_ENABLE, LOW); - pinMode(PIN_MA_PHASE, OUTPUT); - digitalWrite(PIN_MA_PHASE, LOW); - pinMode(PIN_MB_ENABLE, OUTPUT); - digitalWrite(PIN_MB_ENABLE, LOW); - pinMode(PIN_MB_PHASE, OUTPUT); - digitalWrite(PIN_MB_PHASE, LOW); - pinMode(LED_BUILTIN, OUTPUT); - digitalWrite(LED_BUILTIN, LOW); - - /* Dev Stuff */ - if(E_OK == devStuff) - { - /* Say Good night */ - Serial.println("Good night!"); - - delay(100); - } - - /* Go to sleep */ - if(sleepTime & 1u) - { - LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF); - } - if(sleepTime & 2u) - { - LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF); - } - if(sleepTime & 4u) - { - LowPower.powerDown(SLEEP_4S, ADC_OFF, BOD_OFF); - } - - while(sleepTime & 0xFFF8u) - { - sleepTime -= 8; - LowPower.powerDown(SLEEP_8S, ADC_OFF, BOD_OFF); - } - } -} - /*************************************************************************************** * Function: Robot_PowerManagement() *************************************************************************************** * Description: This function checks the Energy Consumption and decide what to do. **************************************************************************************/ -void Robot_PowerManagement() +void Robot_PowerManagement(void) { volatile uint16_t batteryLevel; volatile float batteryVoltage; @@ -515,20 +779,24 @@ void Robot_PowerManagement() **************************************************************************************/ void Robot_Testing(void) { - /* Sleep for 20 seconds to Measure Energy Consumption */ - //Robot_Sleep(20); + static uint32_t batteryReadTimeout = 0; + static float batteryVoltage = 0; - /* Test if motors stopped working */ - //Motor_TestMotor(DRV8834_MOTOR_BOTH); - - /* Read Battery Level */ - float batteryLevel = analogRead(PIN_BATTERY_LEVEL); + /* Read Battery Level once every second */ + if(DELAY_1_SECOND < (millis() - batteryReadTimeout)) + { + /* Update read timeout */ + batteryReadTimeout = millis(); - /* Battery voltage is double the reading value; Voltage Divider is used with R1 = R2 */ - float batteryVoltage = (batteryLevel * (ADC_MAX_VOLTAGE / ADC_MAX_VALUE)) * 2; + /* Battery voltage is double the reading value; Voltage Divider is used with R1 = R2 */ + batteryVoltage = 2 * (analogRead(PIN_BATTERY_LEVEL) * (ADC_MAX_VOLTAGE / ADC_MAX_VALUE)); - /* Show battery level on Serial */ - Serial.println(batteryVoltage); + /* Show battery level on Serial */ + Serial.println(batteryVoltage); + } + + /* Test if motors stopped working */ + //Motor_TestMotor(DRV8834_MOTOR_BOTH); } /*************************************************************************************** @@ -543,6 +811,7 @@ void setup(void) { /* Prepare Debug */ Serial.begin(SERIAL_BRATE); + while(!Serial); } else { diff --git a/Notes.h b/Notes.h index a654081..1886d60 100644 --- a/Notes.h +++ b/Notes.h @@ -28,13 +28,13 @@ * D5 --- Used by Motor B Enable * D6 --- Used by Motor B Phase * D7 --- Used by DRV8834 Sleep Pin - * D8 --- - * D9 --- Used to read IR Receiver + * D8 --- Used by IR Obstacle Data + * D9 --- Used by IR Receiver Data * D10 --- Reserved for SPI SS(CS) (possible E-Paper?) * D11 --- Reserved for SPI MOSI (an E-Paper would be nice) * D12 --- Reserved for SPI MISO (like, you can draw emotions) * D13 --- Reserved for SPI SCK (on the E-Paper) - * A0 --- Used to power IR Receiver + * A0 --- Used to power IR Modules * A1 --- * A2 --- * A3 --- Used to read Battery Level @@ -46,8 +46,8 @@ */ /* ----- DRV8834 ----- - * - CFG pin is wired to GND(maybe not needed?). - * - M1 pin is wired to GND(we don't really need to use it, and Slow Decay is perfect). + * - CFG pin is wired to GND. + * - M1 pin is wired to GND. * - Motors need some amperage, doesn't work powered by TTL Serial Programmer, must use a LiPo Battery or similar. * - In documentation it is mentioned that when waking up from Sleep the Driver might take up to 1ms to become functional. * - Consume aprox 2.5mA when Idle, according to some measurements. @@ -57,13 +57,14 @@ * - Used an old LiPo single cell Battery, looks like a 14500. * - 1200mA or 2200mA one or the other. * - For BMS one small, round, green, unnamed, 1S component was used that is based on 8205A Z1J0802 chip. - * - For Solar Controller an CN3065 Solar Charger v1.0 was used, MPPT doesn't really make sense vor a 6V Panel. + * - For Solar Controller an CN3065 Solar Charger v1.0 was used, MPPT doesn't really make sense for a 6V Panel. * - LiPo connected to BMS -> BMS connected to Solar Charger -> Solar Charger connected to Solar Panel + Robot(Arduino+DRV8834). */ /* ----- Energy Management ----- - * - Voltage measured with Voltage Divider. R1 == R2. Resistence used: 120 Ohm probably 1W. 2W would be better. + * - Voltage measured with Voltage Divider. R1 == R2. Resistence used: 10 kOhm probably less than 1W. 2W would be better. * - Pin Read Voltage drops by 0.05 V when Motors are running. Take in consideration for threshold. + * - TODO: Decide if to desolder IR Obstacle LEDs. There are 2 bright leds, so it must consume some power. * -- Measurements :: * == No Modifications; no Loop Code == * - Startup : jumps to ~5mA, fluctuates to 4 and 6 a little bit, then it jumps to Idle current of 7.03mA @@ -72,10 +73,29 @@ * - Only Motor Driver Asleep : 4.51 mA => Motor Driver in Idle consume aprox 2.5 mA * - Only IR Receiver Asleep : 6.68 mA => IR Receiver in Idle consume aprox 0.4 mA * - Robot Sleeping : 1.58 mA - * == Battery Voltage Reader - * - Pin Read : 3.97 V == Multimeter Reading : 4.01 V => Accuracy up to 0.04 V - * - Pin Read : 3.95 V == Multimeter Reading : 3.99 V => Accuracy up to 0.04 V - * + * == Battery Voltage Reader == + * - Pin Read : 3.97 V == Multimeter Reading : 4.01 V => Accuracy up to 0.04 V (R1 == R2 == 120 Ohm) + * - Pin Read : 3.95 V == Multimeter Reading : 3.99 V => Accuracy up to 0.04 V (R1 == R2 == 120 Ohm) + * - Pin Read : 3.60 V == Multimeter Reading : 3.88 V => Accuracy up to 0.28 V (R1 == R2 == 10 kOhm) + * - Pin Read : 3.87 V == Multimeter Reading : 3.89 V => Accuracy up to 0.02 V (R1 == R2 == 2 kOhm) + * - Pin Read : 3.86 V == Multimeter Reading : 3.88-3.89 V => Accuracy up to 0.03 V (R1 == R2 == 5 kOhm) + * - Pin Read : 3.86 V == Multimeter Reading : 3.89 V => Accuracy up to 0.03V (R1 == R2 == 10 kOhm) + * == IR Obstacle Working; Battery Management Checks == + * - IR No Obstacle : 40.2 mA + * - IR Obstacle : 40.9 mA + * - Jumps to 20.2 for 1 second, then when IR starts working it jumps to 40,6 mA + * - This increase in battery consumption seems to be caused by Voltage Divider. This is not ok. + * - Robot Sleeping : 17.3 mA. Inacceptible! Need Higher Resistence! + * - Changed Resistences from 120 Ohm to 10 kOhm => Sleep at 1.7 mA. Seems to work fine. + * - Unknown Resistence Wattage. Still doesn't heat when Idle/Sleep. + * - Robot Sleeping : 2.4 mA with R = 2 kOhm. Try with 5 kOhm. + * - Robot Sleeping : 1.8 mA with R = 5 kOhm. Best is 10 kOhm but let's see also accuracy. + * - Robot Running : 0.19A, Spike to 0.21 or slightly more + * == Everything Ready == + * - Manual Mode Idle : 11.99 mA + * - Manual Mode Idle(without IR Detector) : 7.45 mA => IR Detector consume aprox 4.5 mA + * I expect ~2 mA if i desolder the LEDs + * - Robot Sleeping : 1.68 mA */ /* ----- IR Remote Control ----- @@ -88,13 +108,22 @@ * Autonomous and Manual. * - In Manual State, it will listen for IR Commands and execute them. * - In Autonomous State it will walk autonomously and avoid obstacles with sensors. - * - If IR is not resumed after reading it it will be stuck with the same value forever. Resume let it read the next command. - * - Consume aprox 0.4mA when Idle, according to some measurements. + * - If IR is not resumed after reading it it will be stuck with the same value forever. Resume to let it read the next command. + * - Consume aprox 0.4 mA when Idle, according to some measurements. */ -/* TODO: Don't be blind +/* ----- Don't be blind ----- * - An IR Distance Sensor will be used to detect objects ahead. * - If an object is detected, the robot will rotate around and try to find another path with no obstacles. + * - IR Output is LOW when there is an obstacle detected, else is HIGH. + * - Will be powered from the same pin as IR Remote. + * - PROBLEM(solved): If the configured threshold is too high the robot will see objects everywhere. At the limit it is reading 2cm ahead... + * This is caused by Ambient Light, sensor reads ok if not in direct light. + * - Changed IR Obstacle Sensor to Ambient Light resistent one(KY-032 KeyesIR 4 pin Sensor). Now it works in ambient light. + * - PROBLEM: The IR Sensor detects in a narrow direction, obstacles too high or too low are not detected. + * Solution 1: Make Robot compact and short. But there are a lot of wires, can't do now. + * Solution to Solution 1: Make a design board or a PCB to make everything compact. + * - Consume aprox 4.5 mA when Idle, according to some measurements. */ /* TODO: Laser Eyes @@ -103,10 +132,11 @@ * - It will be positioned looking down ahead, probably at an angle of 45 or 60 degrees. * - If a the measured distance is higher than a configured threshold then this means there is a hole/stairs ahead * => don't move + * - One alternative solution would be to use a Servo Motor to tilt the IR Obstacle Sensor down. Might consume more tho. */ /* TODO: Function to set Driver Max Current - * - My Motors seems to use 120mA max each. + * - My Motors seems to use 120 mA max each. * - Something that shall be made on Driver Hardware? */ @@ -117,13 +147,11 @@ * - Each Motor has a ENABLE Pin and a PHASE Pin (xENBL and xPHASE). * - xPHASE is controlling the direction, while xENBL controlls the motor. * - xENBL can be used as PWM to controll the motor. + * - PWM doesn't work probably because motors need the 3.something voltage to work. + * - TODO: Rotate Left and Right are not calibrated. Just a little bit calibrated. */ -/* TODO: RotateRight(degree) and RotateLeft(degree) functions - * - Which will move the motors accordingly so the robot can rotate x degrees to the right or left . - */ - -/* ----- Seel like a baby ----- +/* ----- Sleep like a baby ----- * - Robot will sleep while charging and when the battery level is low. * - Motor Driver put into sleep mode by setting the SLEEP Pin to LOW. * - Every other unessential peripheral is powered down.