alpharex.nxc

#include "NXCDefs.h"
#include "alpharex.h"

#define ARMS OUT_A
#define LEFT_LEG OUT_B
#define RIGHT_LEG OUT_C
#define LEFT_DANCE OUT_AB
#define RIGHT_DANCE OUT_AC
#define LEGS OUT_BC
#define LIMBS OUT_ABC

#define MOTOR_SPEED 70
#define UTURN 180
#define DANCE_STEP 54

#define LEFT_DIRECTION 1
#define RIGHT_DIRECTION 2

void init() {
    setLight();
    SetSensorTouch(SENSOR_LEFT);
    SetSensorTouch(SENSOR_RIGHT);
    SetSensorUltrasonic(SENSOR_US);
}

void stopMoving() {Off(LIMBS);}

void stand() {
    stopMoving();
    OnFwd(LEFT_LEG, MOTOR_SPEED);
    until (!SENSOR_LEFT_SENSOR);
    until (SENSOR_LEFT_SENSOR);
    Off(LEFT_LEG);
    OnFwd(RIGHT_LEG, MOTOR_SPEED);
    until (!SENSOR_RIGHT_SENSOR);
    until (SENSOR_RIGHT_SENSOR);
    Off(RIGHT_LEG);
}

void dance(const int left) {
    stand();
    if (left) {
        RotateMotor(RIGHT_LEG, MOTOR_SPEED, DANCE_STEP);
        OnFwd(LEFT_DANCE, MOTOR_SPEED);
    } else {
        RotateMotor(LEFT_LEG, MOTOR_SPEED, DANCE_STEP);
        OnFwd(RIGHT_DANCE, MOTOR_SPEED);
    }
}

void walk(const Direction direction) {
    stand();
    int leg = LEFT_LEG, speed = MOTOR_SPEED;
    if (direction & LEFT_DIRECTION) leg = RIGHT_LEG;
    RotateMotor(leg, speed, UTURN);
    if (direction % 2 == 0) speed = -speed;
    if (direction & (1 >> LEFT_DIRECTION)) OnFwd(LEFT_DANCE, speed);
    else if (direction & (1 >> RIGHT_DIRECTION)) OnFwd(RIGHT_DANCE, speed);
    else OnFwd(LIMBS, speed);
}

void setLight() {SetSensorLight(SENSOR_COLOR);}

void setColor(const Color color) {
    switch (color) {
        case Full: SetSensorType(SENSOR_COLOR, SENSOR_TYPE_COLORFULL);
            break;
        case Red: SetSensorType(SENSOR_COLOR, SENSOR_TYPE_COLORRED);
            break;
        case Green: SetSensorType(SENSOR_COLOR, SENSOR_TYPE_COLORGREEN);
            break;
        case Blue: SetSensorType(SENSOR_COLOR, SENSOR_TYPE_COLORBLUE);
            break;
        default: SetSensorType(SENSOR_COLOR, SENSOR_TYPE_COLORNONE);
    }
}