/* esta funçao serve para mostrar que robô dê a sua prova por concluída, tendo ou não atingido objetivo.
*Para isso os led devem permanecer intermitente, com uma frequência compreendida entre 1 e 5 Hz*/
void Fim(){
Stop_robot();
//esta a frequencia 1hz
while(TRUE){
printStr("TimeOut\n");
leds(0xF);
wait(5);
leds(0x0);
wait(5);
}
}
void program(){
unsigned char Leds=0x01;
while(1){
if (push == 0){
while(push == 0){
leds(Leds);
}
}
else {
leds(Leds);
Leds=Leds >> 1;
}
}
return;
}
//
// Override CThread::run() to show the Faded LEDs
//
virtual void run() {
CBus leds(LED1, LED2, LED3, LED4, END);
while( isAlive() ) { // use the isAlive() mean the task can be kill()
//
// Indicate led in every 20ms
//
if ( m_pulse->fadeRate>0 ) {
leds[0] = LED_ON;
if ( m_pulse->fadeRate>64 ) {
leds[1] = LED_ON;
if ( m_pulse->fadeRate>128 ) {
leds[2] = LED_ON;
if ( m_pulse->fadeRate>196 ) {
leds[3] = LED_ON;
} else {
leds[3] = LED_OFF;
}
} else {
leds[2] = LED_OFF;
}
} else {
leds[1] = LED_OFF;
}
m_pulse->fadeRate -= 16;
} else leds = 0; // turn off all leds
sleep(20);
}
}
int main() {
// Initialize keyboard
keyboard = new Keyboard(&pc);
// Initialize the clocks to some reasonable time
cA.reset();
cA.start();
int startup = rand() % 70 + 30;
Thread::wait(startup);
cV.reset();
cV.start();
// Assign interrupts
as_interrupt.rise(&a_sense);
vs_interrupt.rise(&v_sense);
// Initialize the threads
Thread leds(led_thread);
led_addr = &leds;
Thread display(display_thread);
display_addr = &display;
Thread alarm(alarm_thread);
alarm_addr = &alarm;
Thread keyboard(input_thread);
Thread mode_switch(mode_switch_thread);
Thread pace(pace_thread);
pace_addr = &pace;
while (true) { }
}
void noreturn main(void)
{
init();
initSensors();
for(;;)
{
while(!loopActive)
{
; // wait for next loop
}
readSensors();
attitudeCalculation();
control();
actuate();
triggerSonar();
input();
output();
leds();
ATOMIC_BLOCK(ATOMIC_FORCEON)
{
lastLoopTicks = ticksSinceLoopStart;
loopActive = false;
}
}
}
void main() // Main function
{
badge_setup(); // Call badge setup
oledprint("BUTTONS"); // Large display heading
text_size(SMALL); // Switch to small text
cursor(0, 4); // Small cursor row 4
oledprint("PAD: 6543210"); // Display bit index
cursor(0, 5); // Next line
oledprint("STATE:"); // Display state
cursor(0, 7); // Next line
oledprint("EXIT: Press OSH"); // User prompt to exit
while(1) // States loop
{
states = buttons(); // Load buttons output to states
leds(states); // Light up corresponding LEDs
cursor(7, 5); // Position cursor
oledprint("%07b", states); // Display states as binary number
if(states == 0b1000000) break; // If OSH pressed, exit loop
}
pause(400); // Wait for 4/10 s
text_size(LARGE); // Large text
clear(); // Clear display
oledprint("ALL DONE"); // Done message
}
void init_MMA7660FC(void)
{
int x, y, z;
unsigned char val = 0;
bt_accelInitFlag = 1;
if(!eei2cLockFlag)
{
leds(0b111111);
eei2cLock = locknew();
lockclr(eei2cLock);
eei2cLockFlag = 1;
}
if(!st_eeInitFlag) ee_init();
while(lockset(eei2cLock));
i2c_out(st_eeprom, MMA7660_I2C,
MODE, 1, &val, 1);
i2c_out(st_eeprom, MMA7660_I2C,
INTSU, 1, &val, 1);
i2c_out(st_eeprom, MMA7660_I2C,
SR, 1, &val, 1);
val = 0xC1;
i2c_out(st_eeprom, MMA7660_I2C,
MODE, 1, &val, 1);
i2c_stop(st_eeprom);
lockclr(eei2cLock);
accels(&x, &y, &z);
}
int main(void) {
pool_memadd((uint32_t)pool, sizeof(pool));
#ifdef DEBUG
dbg.start();
#endif
CBus leds(LED1, LED2, END);
CFirmataServer svr;
svr.start("FServer", 256);
// Enter an endless loop
while(1){
//
// Simple demo Code (removable)
//
leds[0] = !leds[0]; // active, LED1 blink
if ( CSocket::getLocalIp() ) { // network ready, LED3 blink
leds[1] = !leds[1];
} else {
leds[1] = LED_OFF;
}
sleep(200);
}
return 0 ;
}
vector<string> readBinaryWatch(int num) {
if (num == 0) {
result.push_back("0:00");
return result;
}
vector<bool> leds(10, false);
Backtracking(leds, 0, num);
return result;
}
void program(){
unsigned char imLed=0x00;
int i=0, j=0;
for(i=0;i<9;i++){
leds(imLed);
imLed=imLed+((unsigned char)pow((double)2,(double)i));
__delay_ms(100);
if (imLed==0xFF){
for (j=9;j>-1;j--){
imLed=imLed-((unsigned char)pow((double)2,(double)((j-1))));
leds(imLed);
__delay_ms(100);
}
}
}
}
void BuildServerRequestErrorLightStrategy::lighting() {
if (innerCounter < countsFor1Sec) {
innerCounter++;
}
else {
yellowLed = !yellowLed;
innerCounter = 0;
}
leds(false, yellowLed, false);
}
void WiFiConnectionErrorLightStrategy::lighting() {
if (innerCounter < countsFor1Sec) {
innerCounter++;
}
else {
isRedNotYellow = !isRedNotYellow;
innerCounter = 0;
}
leds(isRedNotYellow, !isRedNotYellow, false);
}
cv::Mat TrackingDebugDisplay::createAnnotatedBlobImage(
TrackingSystem const &tracking, CameraParameters const &camParams,
cv::Mat const &blobImage) {
cv::Mat output;
blobImage.copyTo(output);
DebugImage img(output);
if (tracking.getNumBodies() == 0) {
/// No bodies - just show blobs.
img.drawStatusMessage(
"No tracked bodies registered, only showing detected blobs",
CVCOLOR_RED);
return output;
}
/// @todo right now, just looks at body 0, target 0 - generalize
auto &body = tracking.getBody(BodyId{0});
auto targetPtr = body.getTarget(TargetId{0});
if (!targetPtr) {
/// No optical target 0 on this body, just show blobs.
img.drawStatusMessage(
"No target registered on body 0, only showing detected blobs",
CVCOLOR_RED);
return output;
}
const auto labelOffset = cv::Point2f(1, 1);
const auto textSize = 0.5;
/// Label each of the blobs.
auto &leds = targetPtr->leds();
for (auto &led : leds) {
img.drawLedLabel(led, CVCOLOR_RED, textSize, labelOffset);
}
if (targetPtr->hasPoseEstimate()) {
/// Reproject the beacons.
Reprojection reproject{*targetPtr, camParams};
auto numBeacons = targetPtr->getNumBeacons();
using size_type = decltype(numBeacons);
for (size_type i = 0; i < numBeacons; ++i) {
auto beaconId = ZeroBasedBeaconId(i);
Eigen::Vector2d imagePoint =
getBeaconReprojection(reproject, *targetPtr, beaconId);
img.drawLedLabel(makeOneBased(beaconId), imagePoint,
CVCOLOR_GREEN, textSize, labelOffset);
}
} else {
img.drawStatusMessage("No video tracker pose for this "
"target, so green reprojection "
"not shown",
CVCOLOR_GRAY);
}
return output;
}
/* ==============================================
main task routine
============================================== */
int main(void) {
pool_memadd((uint32_t) pool, sizeof(pool));
#ifdef DEBUG
dbg.start();
#endif
// Simple demo Code (removable)
CPin led(LED1);
SevenLEDx4 leds(UNO_0, UNO_1, UNO_2, UNO_3, UNO_4, UNO_5, UNO_6, UNO_7, UNO_8, UNO_9, UNO_10, UNO_11);
leds.clear();
leds.start();
for (int i=0; i<16; i++) {
leds[0] = i;
leds[1] = i;
leds[2] = i;
leds[3] = i;
sleep(500);
} // */
// TODO: insert setup code here
fbQuery q;
q.start("ssl", 1024);
q.m_value = 0;
int i, exp, lastValue = 0xFFFFFFFF;
uint8_t v[8];
while (1) {
led = !led;
if ( q.m_value!=lastValue ) {
lastValue = q.m_value;
for (i=8, exp=0; i>0; i--) {
v[i-1] = ((int)(lastValue / pow(10.0f, i-1))) % 10; // 計算個字節的值
if ( exp==0 && v[i-1]>0 ) exp = i; // 記錄最大位數
}
leds.clear();
for (i=1; i<=exp; i++) { // 滾動字幕
leds[0] = v[exp-i];
if (i>1) leds[1] = v[exp-i+1];
if (i>2) leds[2] = v[exp-i+2];
if (i>3) leds[3] = v[exp-i+3];
sleep(500);
}
} // */
sleep(500);
}
return 0;
}
//
// main task
//
int main(void) {
/*************************************************************************
*
* your setup code here
*
**************************************************************************/
//
// LED Demo (can be removed)
//
DBG("Hello I'm in debug mode\n");
CBus leds(LED1, LED2, LED3, LED4, END);
leds.output(); // set all pins as output
usbCDC usb;
usb.enable(); // enable USB core
CSerial uart;
uart.enable(115200); // enable serial port
int i = 0;
uint8_t ch;
while(1) {
/**********************************************************************
*
* your loop code here
*
**********************************************************************/
//
// check uart input
//
if ( uart.readable() ) {
//
//
// show led scripts
leds = led_scripts[i];
i = (i+1) < (int)sizeof(led_scripts) ? i+1 : 0;
usb << uart; // use CStream operator <<
}
//
// check usb input
//
if ( usb.readable() ) {
ch = usb; // use CStream operator uint8_t
uart << ch; // use CStream operator <<
}
}
return 0 ;
}
void WindowUSART::serialReceived()
{
// this->ui->data->setText(serial->readAll());
QByteArray dane;
char a;
dane=serial->readAll();
a=dane.at(0);
this->ui->data->setText(dataToString(a));
leds(a);
}
int main(int argc, char **argv)
{
if (argc < 2) {
std::cerr << "Usage: leds_on pIp" << std::endl;
return 1;
}
const std::string pIp = argv[1];
// Create a proxy to ALLeds.
AL::ALLedsProxy leds(pIp);
// Example showing how to switch on a group
std::string name = "FaceLeds";
leds.on(name);
std::cout << "LEDs from " << name << " group are now on." << std::endl;
return 0;
}
C_RESULT update_wiimote(void)
{
C_RESULT res = C_OK;
static struct cwiid_state state,previous_state;
static int control_mode = 1;
static int valid_domain = 0;
int8_t x, y;
static int start=0;
static int select=0;
static vec3 a={0,0,0},s={0,0,0};
static vec3sph r={0,0,0},pr={0,0,0},rref={0,0,0};
static float pitch=0.0f,roll=0.0f,gaz=0.0f,yaw=0.0f;
float tmp;
static int fly=0;
if (cwiid_get_state(wiimote, &state))
{
fprintf(stderr, "Error getting state\n");
res = C_FAIL;
}
#define SWITCHING(X) ((state.buttons&X) && !(previous_state.buttons&X))
#define RELEASING(X) ((!state.buttons&X) && (previous_state.buttons&X))
#define PRESSED(X) ((state.buttons&X))
static int flag_rumble=0;
#define RUMBLE_ON { if (!flag_rumble) {flag_rumble=1; } }
/* Sets how to use the wiimote */
#define MAXMODE 4
if (SWITCHING(CWIID_BTN_MINUS)) {
control_mode--;
if (control_mode<1) control_mode=MAXMODE;
leds(1<<(control_mode-1));
}
if (SWITCHING(CWIID_BTN_PLUS)) {
control_mode++;
if (control_mode>MAXMODE) control_mode=1;
leds(1<<(control_mode-1));
}
/* Gets gravitation G projection on x,y,z axis */
a.x = - (float32_t) ((((double)state.acc[CWIID_X] - wm_cal.zero[CWIID_X]) /
(wm_cal.one[CWIID_X] - wm_cal.zero[CWIID_X])));
a.y = - (float32_t) ((((double)state.acc[CWIID_Y] - wm_cal.zero[CWIID_Y]) /
(wm_cal.one[CWIID_Y] - wm_cal.zero[CWIID_Y])));
a.z = + (float32_t) ((((double)state.acc[CWIID_Z] - wm_cal.zero[CWIID_Z]) /
(wm_cal.one[CWIID_Z] - wm_cal.zero[CWIID_Z])));
s.x = (a.x<0.0f)?(1.0f):(-1.0f);
s.y = (a.y<0.0f)?(1.0f):(-1.0f);
s.z = (a.z<0.0f)?(1.0f):(-1.0f);
float ax2 = a.x*a.x;
float ay2 = a.y*a.y;
float az2 = a.z*a.z;
r.r = sqrtf((ax2+ay2)+az2);
switch(control_mode)
{
case 1:
case 2:
if (r.r==0.0f) { r.p=r.t=0.0f; } else {
// Angle gauche/droite
r.p = asin(a.y); if (isnan(r.p)) r.p=0.0f;
// Sur plan vertical radial
r.t = acos(a.z/(sqrtf(az2+ax2))); if (isnan(r.t)) r.t=0.0f;
r.t*=s.x;
}
break;
case 3:
case 4:
if (r.r==0.0f) { r.p=r.t=0.0f; } else {
// Angle entre le projete de G sur le plan vertical longitudinal (yz) et l'axe z
r.p = acos(a.z/(sqrtf(az2+ay2))); if (isnan(r.p)) r.p=0.0f;
/* If wiimote faces the ground */
//if (a.z<0.0f) r.p= M_PI-r.p;
r.p*=s.y;
// Idem sur le plan vertical radial
r.t = acos(a.z/(sqrtf(az2+ax2))); if (isnan(r.t)) r.t=0.0f;
r.t*=s.x;
}
break;
}
r.r = (r.r+pr.r)/2.0f;
r.t = (r.t+pr.t)/2.0f;
r.p = (r.p+pr.p)/2.0f;
switch(control_mode)
{
case 1:
case 2:
/* Wiimote is handled horizontally.
* '2' button under left thumb
* directionnal cross under right thumb
*/
/* 0 -> buttons facing sky */
if ((SWITCHING(CWIID_BTN_1)||SWITCHING(CWIID_BTN_2)||SWITCHING(CWIID_BTN_B))){ rref=r; }
if (PRESSED(CWIID_BTN_1)||PRESSED(CWIID_BTN_2)||PRESSED(CWIID_BTN_B))
{
/* If wiimote facing ground */
if (a.z<0 && a.x>0)
{
rumble(1);
}
else
{
rumble(0);
leds(1<<(control_mode-1));
pitch = (r.t-rref.t)*1.0f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
roll = -(r.p-rref.p)*0.75f; if (roll<-1.0f) roll=-1.0f; if (roll>1.0f) roll=1.0f;
fly=1;
}
}
else
{
pitch=roll=0;
rumble(0);
leds(1<<(control_mode-1));
fly=0;
}
gaz = (PRESSED(CWIID_BTN_LEFT))? 1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? -1.0f : 0.0f;
yaw = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? 1.0f : (control_mode==2) ? (-pitch*roll) : 0.0f;
break;
case 3:
case 4:
if (PRESSED(CWIID_BTN_B))
{
if (a.z<-0.5f)
{
rumble(1);
}
else
{
rumble(0);
leds(1<<(control_mode-1));
pitch = -(r.p-rref.p)*1.5f; if (pitch<-1.0f) pitch=-1.0f; if (pitch>1.0f) pitch=1.0f;
roll = (r.t-rref.t)*1.5f; if (roll<-1.0f) roll=-1.0f; if (roll>1.0f) roll=1.0f;
fly=1;
}
}
else
{
rumble(0);
leds(1<<(control_mode-1));
fly=0;
pitch=roll=0;
}
if (SWITCHING(CWIID_BTN_B)) { rref = r; }
gaz = (PRESSED(CWIID_BTN_DOWN))? -1.0f: (PRESSED(CWIID_BTN_UP)) ? +1.0f : 0.0f;
yaw = (PRESSED(CWIID_BTN_LEFT))? -1.0f: (PRESSED(CWIID_BTN_RIGHT)) ? 1.0f : (control_mode==4) ? (-pitch*roll) : 0.0f;
}
/* Buttons common to all modes */
if (SWITCHING(CWIID_BTN_A)) { start^=1; ardrone_tool_set_ui_pad_start(start); }
if (SWITCHING(CWIID_BTN_HOME)) {
select^=1;
ardrone_tool_set_ui_pad_select(select);
ardrone_tool_set_ui_pad_start(0);
}
//
//printf("Wiimote mode 2 [ax %f][ay %f][az %f]\n\033[1A", a.x,a.y,a.z);
printf("Wiimode %i [rr %3.2f][rp %3.2f][rt %3.2f]\n", control_mode,r.r,r.p,r.t);
printf("[Fly %i][Ptc %3.2f][Rl %3.2f][Yw %3.2f][Gz %3.2f][Strt. %i][Sel. %i]\n\033[2A", fly,pitch,roll,yaw,gaz,start,select);
ardrone_at_set_progress_cmd( fly,/*roll*/roll,/*pitch*/pitch,/*gaz*/gaz,/*yaw*/yaw);
/*api_set_iphone_acceleros(
(state.buttons&CWIID_BTN_2 ? 1 : 0)|(state.buttons&CWIID_BTN_A ? 2 : 0),
a_x, a_y, a_z);*/
previous_state = state;
pr=r;
return C_OK;
}
cv::Mat TrackingDebugDisplay::createStatusImage(
TrackingSystem const &tracking, CameraParameters const &camParams,
cv::Mat const &baseImage, bool reprojectUnseenBeacons) {
cv::Mat output;
DebugImage img(output);
baseImage.copyTo(output);
if (tracking.getNumBodies() == 0) {
/// No bodies - show a message and swit
img.drawStatusMessage("No tracked bodies registered, "
"showing raw input image - press "
"b to show blobs",
CVCOLOR_RED);
return output;
}
/// @todo right now, just looks at body 0, target 0 - generalize
auto &body = tracking.getBody(BodyId{0});
auto targetPtr = body.getTarget(TargetId{0});
if (!targetPtr) {
/// No optical target 0 on this body
img.drawStatusMessage("No target registered on body 0, "
"showing raw input image - press "
"b to show blobs",
CVCOLOR_RED);
return output;
}
/// OK, so if we get here, we have a valid target, so we can start with
/// the base image as we'd like.
const auto textSize = 0.25;
const auto mainBeaconLabelColor = CVCOLOR_BLUE;
const auto baseBeaconLabelColor =
m_performingOptimization ? CVCOLOR_WHITE : CVCOLOR_BLACK;
auto gotPose = targetPtr->hasPoseEstimate();
auto numBeacons = targetPtr->getNumBeacons();
using BeaconIdContainer = std::vector<ZeroBasedBeaconId>;
auto drawnBeaconIds = BeaconIdContainer{};
auto recordBeaconAsDrawn = [&](ZeroBasedBeaconId id) {
drawnBeaconIds.push_back(id);
};
/// Unidentified blobs look the same whether or not we have a pose, so
/// we make a little lambda here to avoid repeating ourselves too much.
auto drawUnidentifiedBlob = [&img](Led const &led) {
/// Red empty circle for un-identified blob
img.drawLedCircle(led, false, CVCOLOR_RED);
};
if (gotPose) {
/// We have a pose - so we'll reproject identified beacons.
Reprojection reproject{*targetPtr, camParams};
/// Draw axes.
drawOriginAxes(reproject, img);
for (auto const &led : targetPtr->leds()) {
if (led.identified()) {
/// Identified, and we have a pose
auto beaconId = led.getID();
recordBeaconAsDrawn(beaconId);
// Color-code identified beacons based on
// whether or not we used their data.
auto color =
led.wasUsedLastFrame() ? CVCOLOR_GREEN : CVCOLOR_YELLOW;
img.drawLedCircle(led, true, color);
/// Draw main label in black, then draw the reprojection in
/// blue on top, creating a bit of a shadow effect. Not
/// best visiblity ever, but...
/// label at keypoint location
img.drawLedLabel(led, baseBeaconLabelColor, textSize);
/// label at reprojection
Eigen::Vector2d imagePoint =
getBeaconReprojection(reproject, *targetPtr, beaconId);
auto windowPoint =
WindowCoordsPoint{eigenVecToPoint(imagePoint)};
img.drawLedLabel(makeOneBased(beaconId), imagePoint,
mainBeaconLabelColor, textSize);
} else {
drawUnidentifiedBlob(led);
}
}
if (reprojectUnseenBeacons) {
/// Sort so we can use binary_search.
auto comparator = [](ZeroBasedBeaconId const &a,
ZeroBasedBeaconId const &b) {
return a.value() < b.value();
};
std::sort(begin(drawnBeaconIds), end(drawnBeaconIds),
comparator);
auto haveDrawnBeaconAlready = [&](ZeroBasedBeaconId id) {
return std::binary_search(begin(drawnBeaconIds),
end(drawnBeaconIds), id,
comparator);
};
/// Now, we must draw reprojections of the unseen beacons.
auto numBeacons = targetPtr->getNumBeacons();
for (UnderlyingBeaconIdType i = 0; i < numBeacons; ++i) {
auto beaconId = ZeroBasedBeaconId(i);
if (haveDrawnBeaconAlready(beaconId)) {
/// already drawn - so skip it.
continue;
}
/// label at reprojection
Eigen::Vector2d imagePoint =
getBeaconReprojection(reproject, *targetPtr, beaconId);
auto windowPoint =
WindowCoordsPoint{eigenVecToPoint(imagePoint)};
img.drawLedLabel(makeOneBased(beaconId), imagePoint,
CVCOLOR_VIOLET, textSize);
}
}
} else {
/// If we don't have a pose...
for (auto const &led : targetPtr->leds()) {
if (led.identified()) {
// If identified, but we don't have a pose, draw
// them as yellow outlines.
img.drawLedCircle(led, false, CVCOLOR_YELLOW);
img.drawLedLabel(led, baseBeaconLabelColor, textSize);
} else {
drawUnidentifiedBlob(led);
}
}
}
return output;
}
/*! @brief Displays the battery's charge and the (dis)charge rate on the NAO's ears
*/
bool NAOPlatform::displayBatteryState()
{
float currenttime = Blackboard->Sensors->CurrentTime;
float period = currenttime - m_battery_state_previous_time;
// get the battery charge and voltage from the sensor data
float charge, current;
Blackboard->Sensors->getBatteryCharge(charge);
Blackboard->Sensors->getBatteryCurrent(current);
int numleds = 10;
vector<float> leds(numleds, 0);
// calculate the number of lights to turn on in the ears based on the battery charge
int numon = (int) (charge*numleds + 0.5);
for (int i=0; i<numon; i++)
leds[i] = 1;
Blackboard->Actions->add(NUActionatorsData::LEarLed, currenttime, leds);
Blackboard->Actions->add(NUActionatorsData::REarLed, currenttime, leds);
// do the fancy animated charging/discharging displays
if (current >= 0 and numon < numleds)
{ // the battery is charging
int loops = current/0.5 + 1;
double timeperloop = period/loops;
double slope = timeperloop/(numleds - numon);
for (int l=0; l<loops; l++)
{
for (int i=numon; i<numleds; i++)
{
vector<float> chargeleds = leds;
chargeleds[i] = 1;
Blackboard->Actions->add(NUActionatorsData::LEarLed, currenttime + slope*(i+1 - numon) + l*timeperloop, chargeleds);
Blackboard->Actions->add(NUActionatorsData::REarLed, currenttime + slope*(i+1 - numon) + l*timeperloop, chargeleds);
}
}
}
else if (numon > 1)
{ // the battery is discharging
int loops = -current/1.0 + 1;
double timeperloop = period/loops;
double slope = timeperloop/numon;
for (int l=0; l<loops; l++)
{
for (int i=0; i<numon; i++)
{
vector<float> chargeleds = leds;
chargeleds[(numon-1) - i] = 0;
Blackboard->Actions->add(NUActionatorsData::LEarLed, currenttime + slope*(i+1) + l*timeperloop, chargeleds);
Blackboard->Actions->add(NUActionatorsData::REarLed, currenttime + slope*(i+1) + l*timeperloop, chargeleds);
}
}
}
// say low_battery.wav when the battery is really low
bool ok = true;
if (charge < 0.02 and current < 0 and currenttime - m_battery_voiced_time > 5000)
{
Blackboard->Actions->add(NUActionatorsData::Sound, currenttime, "low_battery.wav");
m_battery_voiced_time = currenttime;
ok = false;
}
m_battery_state_previous_time = currenttime;
return ok;
}
//------------------------------------------------------------------------
// Sample program to demonstrate the LCD and keypad
//------------------------------------------------------------------------
void main ( void )
{
auto int option;
static int state;
//------------------------------------------------------------------------
// Initialize the controller
//------------------------------------------------------------------------
brdInit(); // Initialize the controller
dispInit(); // Start-up the keypad driver, Initialize the graphic driver
keypadDef(); // Use the default keypad ASCII return values
glBackLight(1); // Turn-on the backlight
glXFontInit(&fi6x8, 6, 8, 32, 127, Font6x8); // Initialize 6x8 font
glXFontInit(&fi8x10, 8, 10, 32, 127, Font8x10); // Initialize 10x16 font
glXFontInit(&fi12x16, 12, 16, 32, 127, Font12x16); // Initialize 12x16 font
// Setup and center text window to be the entire display
TextWindowFrame(&textWindow, &fi6x8, 1, 0, 121, 32);
// Set variables to known states
ledCntrl = LEDOFF; // Initially disable the LED's
state = MENU_INIT;
//------------------------------------------------------------------------
// Display Sign-on message and wait for keypress
//------------------------------------------------------------------------
SignOnMessage();
//------------------------------------------------------------------------
// Main program loop for the MENU system
//------------------------------------------------------------------------
for (;;)
{
costate
{
keyProcess ();
waitfor(DelayMs(10));
}
costate
{
leds(OPERATE);
waitfor(DelayMs(50));
}
costate
{
// Display the MAIN MENU
waitfor((option = display_menu(main_menu, &state, LVL_MAINMENU, NUM_MAINMENU_OPTS)) > 0);
// Get menu option from the user
switch(option)
{
// Change Date/Time
case 1: glBlankScreen();
SetDateTime();
state = MENU_INIT;
break;
// Display current Date/Time
case 2: glBlankScreen();
waitfor(dispDate());
state = MENU_INIT;
break;
// Display backlight memu options
case 3: waitfor(backlight_menu());
state = MENU_REFRESH;
break;
// Enable Toggle leds option
case 4: leds(TOGGLE);
state = MENU_NO_CHANGE;
break;
// Enable Increment leds option
case 5: leds(INCREMENT);
state = MENU_NO_CHANGE;
break;
// Disable LED's
case 6: leds(LEDOFF);
state = MENU_NO_CHANGE;
break;
// User made invalid selection
default:
break;
}
}
}
}
//
// main task
//
int main(void) {
#ifdef DEBUG
#if __USE_USB
usbCDC ser;
ser.connect();
#else
CSerial ser;
ser.settings(115200);
#endif
CDebug dbg(ser);
dbg.start();
#endif
CBus leds(LED1, LED2, LED3, LED4, END);
//
// your setup code here
//
myPulseSensor pulse(AD0);
pulse.start();
CTimeout t1, t2;
//
// your loop code here
//
while(1) {
//
// Indicate led in every 20ms
//
if ( t1.isExpired(20) ) {
t1.reset();
if ( pulse.fadeRate>0 ) {
leds[0] = LED_ON;
if ( pulse.fadeRate>64 ) {
leds[1] = LED_ON;
if ( pulse.fadeRate>128 ) {
leds[2] = LED_ON;
if ( pulse.fadeRate>196 ) {
leds[3] = LED_ON;
} else {
leds[3] = LED_OFF;
}
} else {
leds[2] = LED_OFF;
}
} else {
leds[1] = LED_OFF;
}
pulse.fadeRate -= 16;
} else leds = 0; // turn off all leds
}
//
// Show BPM in every 1 second
//
if ( t2.isExpired(1000) ) {
t2.reset();
DBG("BPM=%d\n", pulse.BPM);
}
}
return 0 ;
}
int main (void)
{
int countCiclos = 100;
//iniciçao da pic
initPIC32 ();
closedLoopControl( true );
setVel2(0, 0);
printStr(__FILE__); // para saber o nome do ficheiro que esta a correr no robot
printf("\r battery: %d ", batteryVoltage());
if(batteryVoltage() <94){
printf("_Bateria fraca, MUDAR Bateria\n");
}
printStr("\n");
while(1)
{
while(!tick40ms);
tick40ms=0;
readAnalogSensors();
//state buttons
if(startButton() == 1) // Botao start(preto) primido
{
estado = 1;
enableObstSens();
leds(0x0); //leds off
countCiclos = 100;
}
else if(stopButton() == 1) //Botao stop(vermelho) primido
{
printf("Red button pressed!!! \n");
estado = 0;
disableObstSens();
}
if(estado == 1)
{
TimeOut(); // timeOut => tb devia ir para uma inturrupcao
Chegada_Farol();
if(countCiclos++ >= 100)
{
Ver_Farol();
countCiclos = 0;
}
Run_Beacon();
}
if(estado == 2)
{
leds(0x1);
Stop_robot();
}
if(estado == 3)
Fim();
else if(stopButton() == 1 || estado == 0) // deslica o funcionamento, nenhum led activo
{
Stop_robot();
}
}
return (0);
}
int main (void)
{
//iniciçao da pic
initPIC32 ();
closedLoopControl( true );
setVel2(0, 0);
T4CONbits.TCKPS=5;
PR4=(PBCLK/32/3)-1;//fout 20MHz/(32*(62499+1))*10Hz,freq a 1 hz
TMR4=0;//Reset timer T2
T4CONbits.TON=1;
//Interrupts
IFS0bits.T4IF=0;//reset do timer
IPC4bits.T4IP=2;//priority
IEC0bits.T4IE=1;//enable timer3
EnableInterrupts();
printStr(__FILE__); // para saber o nome do ficheiro que esta a correr no robot
printf("\r battery: %d ", batteryVoltage());
printStr("\n");
//desligar os leds para o inicio
leds(0b1111); //leds off
#if 0
while(1)
{
while(!tick40ms);
tick40ms=0;
readAnalogSensors();
}
#endif
while (TRUE)
{
TimeOut(); // timeOut => tb devia ir para uma inturrupcao
//Sensor(); //leitura os sensores tem de se por numa interropecao
//botao de estado
if(startButton() == 1) // Botao start(preto) primido
{
estado = 1;
resetCoreTimer();
leds(0x1);
}
else if(stopButton() == 1) //Botao stop(vermelho) primido
{
estado = 0;
leds(0x0);
}
//funcoa de funcionameneto
if(estado == 1) // robot fica em presesamento
{
Ver_Farol();
printf("D: %d F: %d E: %d LINHA: %d Farol: %d \r", sensor_dir,sensor_frente,sensor_esq,linha ,farolsen );
ANDAR2();
}
//quando e primido o botao stop
else if(stopButton() == 1) // deslica o funcionamento, nenhum led activo
{
Stop_robot();
}
}
return (0);
}
int main()
{
// PC connection
Serial pc(0, 9600);
// Turn all leds on while waiting
Bus leds(4, LED4, LED3, LED2, LED1);
leds.mode(Output);
leds.write_all(1);
// Config motors
Motor motorB (P22, P21, P23);
Motor motorA (P25, P24, P26);
// Turn lights on
FarolA = 1;
FarolB = 1;
// The interface with the remote control
Serial cmdLink(1, 38400);
// Read until the control says START
wait_for_start(cmdLink);
// Seteo velocidad y direccion del motor A
motorA.set_sentido(ADELANTE);
motorA.Velocidad = MARCHA_IZQ;
// Seteo velocidad y direccion del motor B
motorB.set_sentido(ADELANTE);
motorB.Velocidad = MARCHA_DER;
while (1)
{
// encendemos los leds de acuerdo con los sensores
//estado_sensores();
unsigned char cmd = get_command(cmdLink);
leds.write(cmd);
// Stop makes the car stop until the next start
if (cmd == STOP) {
motorA.set_sentido(DETENIDO);
motorB.set_sentido(DETENIDO);
wait_for_start(cmdLink);
}
// Go faster
else if (cmd == START) {
motorA.Velocidad = 0.9;
motorB.Velocidad = 0.9;
}
// Enter line_follower mode
else if (cmd == LINE_FOLLOWER) {
line_follower(motorA, motorB);
}
// Go forward
else if (cmd == (CENTER+FORWARD)) {
motorA.set_sentido(ADELANTE);
motorA.Velocidad = MARCHA_IZQ;
motorB.set_sentido(ADELANTE);
motorB.Velocidad = MARCHA_DER;
}
// Go backward
else if (cmd == (CENTER+BACKWARD)) {
motorA.set_sentido(ATRAS);
motorA.Velocidad = MARCHA_IZQ;
motorB.set_sentido(ATRAS);
motorB.Velocidad = MARCHA_DER;
}
// Go left
else if (cmd > LEFT && cmd < RIGHT) {
motorB.set_sentido(ADELANTE);
float f = (cmd - LEFT) / 10.;
if (f > 0.7) {
motorA.set_sentido(ATRAS);
motorA.Velocidad = MARCHA_IZQ*(f-0.2);
motorB.Velocidad = MARCHA_DER*(f-0.2);
} else {
motorA.set_sentido(ADELANTE);
motorA.Velocidad = MARCHA_IZQ*(0.7-f);
motorB.Velocidad = MARCHA_DER*(1+f);
}
}
// Go right
else if (cmd > RIGHT && cmd < BACKWARD) {
motorA.set_sentido(ADELANTE);
float f = (cmd - RIGHT) / 10.;
if (f > 0.7) {
motorB.set_sentido(ATRAS);
motorA.Velocidad = MARCHA_IZQ*(f-0.2);
motorB.Velocidad = MARCHA_DER*(f-0.2);
} else {
motorB.set_sentido(ADELANTE);
motorA.Velocidad = MARCHA_IZQ*(1+f);
motorB.Velocidad = MARCHA_DER*(0.7-f);
}
} else {
cmd -= BACKWARD;
if (cmd > LEFT && cmd < RIGHT) {
float f = (cmd - LEFT) / 10.;
motorB.set_sentido(ATRAS);
if (f > 0.7) {
motorA.set_sentido(ADELANTE);
motorA.Velocidad = MARCHA_IZQ*(f-0.2);
motorB.Velocidad = MARCHA_DER*(f-0.2);
} else {
motorA.set_sentido(ATRAS);
motorA.Velocidad = MARCHA_IZQ*(0.9-f);
motorB.Velocidad = MARCHA_DER*(1+f);
}
} else {
float f = (cmd - RIGHT) / 10.;
motorA.set_sentido(ATRAS);
if (f > 0.7) {
motorB.set_sentido(ADELANTE);
motorA.Velocidad = MARCHA_IZQ*(f-0.2);
motorB.Velocidad = MARCHA_DER*(f-0.2);
} else {
motorB.set_sentido(ATRAS);
motorA.Velocidad = MARCHA_IZQ*(1+f);
motorB.Velocidad = MARCHA_DER*(0.9-f);
}
}
}
}
}
