Ben, ça marche pas
This commit is contained in:
parent
0b1b1b6130
commit
fde6592dec
275
IO.hpp
275
IO.hpp
|
|
@ -1,33 +1,91 @@
|
|||
#include "esp32-hal.h"
|
||||
#include <CodeCell.h>
|
||||
#include <ESP32Servo.h>
|
||||
#include <Adafruit_SSD1306.h>
|
||||
#include <cmath>
|
||||
|
||||
#define PIN_TIRETTE 7
|
||||
#define PIN_BUTTON_COLOR 6
|
||||
#define PIN_MOTOR1 1
|
||||
#define PIN_MOTOR2 2
|
||||
#define PIN_SERVO 5
|
||||
|
||||
class Motor;
|
||||
#define DANCING_ACTION_DELTA_ANGLE 2
|
||||
|
||||
class Caca {
|
||||
Caca(int quatitee) : kg(quantitee)
|
||||
{}
|
||||
#define KD 100
|
||||
#define ANGULAR_SPEED 45 // °/s
|
||||
|
||||
int kg;
|
||||
enum class Axes {
|
||||
X,
|
||||
Y,
|
||||
Z
|
||||
};
|
||||
|
||||
Caca my_caca {21};
|
||||
|
||||
class IO {
|
||||
class Motor {
|
||||
public:
|
||||
int init()
|
||||
int init(int pin)
|
||||
{
|
||||
pinMode(PIN_TIRETTE, INPUT_PULLUP);
|
||||
m_pin = pin;
|
||||
|
||||
for(int i {0}; i < 2; i++)
|
||||
{
|
||||
motors[i].init();
|
||||
}
|
||||
pinMode(m_pin, OUTPUT);
|
||||
|
||||
analogWriteFrequency(m_pin, 5000);
|
||||
analogWriteResolution(m_pin, 12);
|
||||
|
||||
analogWrite(m_pin, 0);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void setSpeed(int speed)
|
||||
{
|
||||
Serial.printf("Motor pin : %d\n", m_pin);
|
||||
analogWrite(m_pin, speed);
|
||||
}
|
||||
|
||||
private:
|
||||
int m_pin;
|
||||
int m_channel;
|
||||
};
|
||||
|
||||
class IO {
|
||||
public:
|
||||
IO(CodeCell *code_cell) : m_code_cell(code_cell)
|
||||
{}
|
||||
|
||||
int init()
|
||||
{
|
||||
int nb_errors {0};
|
||||
|
||||
pinMode(PIN_TIRETTE, INPUT_PULLUP);
|
||||
pinMode(PIN_BUTTON_COLOR, INPUT_PULLUP);
|
||||
|
||||
m_code_cell->Motion_RotationRead(m_init_x, m_init_y, m_init_z);
|
||||
|
||||
m_initMotors();
|
||||
m_initServo();
|
||||
|
||||
nb_errors += m_initScreen();
|
||||
|
||||
return nb_errors;
|
||||
}
|
||||
|
||||
int update()
|
||||
{
|
||||
bool m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
|
||||
m_is_tirette_pulled = (digitalRead(PIN_TIRETTE) == LOW);
|
||||
m_is_color_blue = (digitalRead(PIN_BUTTON_COLOR) == HIGH);
|
||||
|
||||
if(m_is_motor_control_activated)
|
||||
{
|
||||
m_updateMotorsControl();
|
||||
}
|
||||
|
||||
if(m_is_dancing)
|
||||
{
|
||||
m_updateDancingAction();
|
||||
}
|
||||
|
||||
m_updateScreen();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -37,16 +95,183 @@ class IO {
|
|||
return m_is_tirette_pulled;
|
||||
}
|
||||
|
||||
private:
|
||||
bool m_is_tirette_pulled {false};
|
||||
|
||||
Motor motors[2];
|
||||
};
|
||||
|
||||
class Motor {
|
||||
int init()
|
||||
bool isColorBlue()
|
||||
{
|
||||
|
||||
return m_is_color_blue;
|
||||
}
|
||||
|
||||
void motorControlOn()
|
||||
{
|
||||
m_is_motor_control_activated = true;
|
||||
}
|
||||
|
||||
void motorControlOff()
|
||||
{
|
||||
m_is_motor_control_activated = false;
|
||||
|
||||
m_motors[0].setSpeed(0);
|
||||
m_motors[1].setSpeed(0);
|
||||
}
|
||||
|
||||
void setDir(float dir)
|
||||
{
|
||||
static unsigned long prev_time {millis()};
|
||||
float err_dir = dir - m_dir;
|
||||
float max_angular_displacement = ANGULAR_SPEED * (millis() - prev_time);
|
||||
|
||||
if(std::abs(err_dir) < max_angular_displacement)
|
||||
{
|
||||
m_dir = dir;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_dir += max_angular_displacement * (err_dir < 0) ? -1 : 1;
|
||||
}
|
||||
|
||||
prev_time = millis();
|
||||
}
|
||||
|
||||
void setSpeed(float speed)
|
||||
{
|
||||
m_speed = speed;
|
||||
}
|
||||
|
||||
float getAngle(Axes axis)
|
||||
{
|
||||
float x, y, z;
|
||||
|
||||
m_code_cell->Motion_RotationRead(x, y, z);
|
||||
|
||||
switch(axis)
|
||||
{
|
||||
case Axes::X:
|
||||
return x - m_init_x;
|
||||
|
||||
case Axes::Y:
|
||||
return y - m_init_y;
|
||||
|
||||
default:
|
||||
return z - m_init_z;
|
||||
}
|
||||
}
|
||||
|
||||
void startDancingAction()
|
||||
{
|
||||
m_is_dancing = true;
|
||||
}
|
||||
|
||||
Adafruit_SSD1306 *getScreen()
|
||||
{
|
||||
return &m_screen;
|
||||
}
|
||||
|
||||
Motor m_motors[2];
|
||||
|
||||
private:
|
||||
// Init Motors
|
||||
void m_initMotors()
|
||||
{
|
||||
m_motors[0].init(PIN_MOTOR1);
|
||||
m_motors[1].init(PIN_MOTOR2);
|
||||
}
|
||||
|
||||
// Init Servo
|
||||
void m_initServo()
|
||||
{
|
||||
m_servo.setPeriodHertz(50);
|
||||
m_servo.attach(PIN_SERVO, 500, 2400);
|
||||
|
||||
m_servo.write(87);
|
||||
}
|
||||
|
||||
int m_initScreen()
|
||||
{
|
||||
if(!m_screen.begin(SSD1306_SWITCHCAPVCC, 0x3C))
|
||||
{
|
||||
Serial.println("SSD1306 allocation failed.");
|
||||
return -1;
|
||||
}
|
||||
|
||||
m_screen.fillScreen(SSD1306_BLACK);
|
||||
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
|
||||
void m_updateDancingAction()
|
||||
{
|
||||
static unsigned long prev_time {millis()};
|
||||
static unsigned long delta_time {0};
|
||||
static int actual_angle {87};
|
||||
static int8_t actual_dir {-1};
|
||||
|
||||
delta_time += millis() - prev_time;
|
||||
|
||||
if(delta_time >= 10)
|
||||
{
|
||||
delta_time = 0;
|
||||
|
||||
actual_angle += actual_dir * DANCING_ACTION_DELTA_ANGLE;
|
||||
}
|
||||
|
||||
if(actual_angle <= 43)
|
||||
{
|
||||
actual_dir = 1;
|
||||
}
|
||||
else if(actual_angle >= 130)
|
||||
{
|
||||
actual_dir = -1;
|
||||
}
|
||||
|
||||
m_servo.write(actual_angle);
|
||||
|
||||
prev_time = millis();
|
||||
}
|
||||
|
||||
void m_updateMotorsControl()
|
||||
{
|
||||
float actual_angle = getAngle(Axes::Z);
|
||||
float error = m_dir - actual_angle;
|
||||
|
||||
float correction = error * KD;
|
||||
|
||||
int m1_speed = static_cast<int>(m_speed + correction);
|
||||
int m2_speed = static_cast<int>(m_speed - correction);
|
||||
|
||||
if(m1_speed < 0)
|
||||
{
|
||||
m1_speed = 0;
|
||||
m2_speed = static_cast<int>(m_speed - 2 * correction);
|
||||
}
|
||||
else if(m2_speed < 0)
|
||||
{
|
||||
m2_speed = 0;
|
||||
m1_speed = static_cast<int>(m_speed + 2 * correction);
|
||||
}
|
||||
|
||||
Serial.printf("M1_Speed : %d\n", m1_speed);
|
||||
Serial.printf("M2_Speed : %d\n", m2_speed);
|
||||
|
||||
m_motors[0].setSpeed(m1_speed);
|
||||
m_motors[1].setSpeed(m2_speed);
|
||||
}
|
||||
|
||||
void m_updateScreen()
|
||||
{
|
||||
m_screen.display();
|
||||
}
|
||||
|
||||
bool m_is_motor_control_activated {false};
|
||||
bool m_is_tirette_pulled {false};
|
||||
bool m_is_color_blue {false};
|
||||
bool m_is_dancing {false};
|
||||
|
||||
Servo m_servo;
|
||||
|
||||
Adafruit_SSD1306 m_screen {128, 64, &Wire, -1};
|
||||
|
||||
CodeCell *m_code_cell;
|
||||
|
||||
float m_init_x, m_init_y, m_init_z;
|
||||
float m_dir {0};
|
||||
float m_speed {0};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -0,0 +1,64 @@
|
|||
namespace Imgs
|
||||
{
|
||||
constexpr const unsigned char riombotique [] PROGMEM = {
|
||||
0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0xf0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xff, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0x03, 0xe0, 0x00,
|
||||
0x00, 0x00, 0x00, 0xc0, 0x00, 0x38, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00,
|
||||
0x02, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x04, 0x00, 0x07, 0x86, 0x00, 0x00, 0x00, 0x04, 0x06,
|
||||
0x0f, 0xc2, 0x00, 0x00, 0x00, 0x04, 0x19, 0x98, 0x62, 0x00, 0x00, 0x00, 0x04, 0x20, 0x50, 0x22,
|
||||
0x00, 0x00, 0x00, 0x14, 0x20, 0x50, 0x22, 0xc0, 0x00, 0x00, 0x34, 0x20, 0x50, 0x22, 0xe0, 0x00,
|
||||
0x00, 0x74, 0x20, 0x58, 0x62, 0xa0, 0x00, 0x00, 0xf4, 0x10, 0x8f, 0xc2, 0xb0, 0x00, 0x00, 0xf4,
|
||||
0x0f, 0x07, 0x82, 0xb0, 0x00, 0x00, 0x74, 0x00, 0x00, 0x02, 0xa0, 0x00, 0x00, 0x34, 0x00, 0x00,
|
||||
0x02, 0xc0, 0x00, 0x00, 0x14, 0x00, 0x00, 0x02, 0x80, 0x00, 0x00, 0x04, 0x00, 0x00, 0x02, 0x00,
|
||||
0x00, 0x00, 0x04, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00,
|
||||
0x03, 0xff, 0xff, 0xf8, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x02, 0x00,
|
||||
0x00, 0x0c, 0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x01,
|
||||
0x00, 0x00, 0x00, 0x08, 0x07, 0xff, 0x00, 0x80, 0x00, 0x00, 0x18, 0x07, 0xff, 0x00, 0x80, 0x00,
|
||||
0x01, 0x98, 0x07, 0xff, 0x00, 0xc0, 0x00, 0x03, 0xd8, 0x00, 0x00, 0x00, 0xc4, 0x00, 0x03, 0x58,
|
||||
0x00, 0x00, 0x00, 0xde, 0x00, 0x02, 0x38, 0x00, 0x00, 0x00, 0xfa, 0x00, 0x02, 0x38, 0x00, 0x00,
|
||||
0x00, 0xe2, 0x00, 0x06, 0x78, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x04, 0x78, 0x00, 0x00, 0x00, 0xe1,
|
||||
0x00, 0x04, 0x78, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x0c, 0x78, 0x00, 0x00, 0x00, 0xe1, 0x00, 0x08,
|
||||
0xf8, 0x00, 0x00, 0x00, 0xf0, 0x80, 0x08, 0xf8, 0x00, 0x00, 0x00, 0xf0, 0x80, 0x08, 0x98, 0x00,
|
||||
0x00, 0x00, 0xf0, 0x80, 0x11, 0x98, 0x00, 0x00, 0x00, 0xf0, 0x40, 0x19, 0x88, 0x00, 0x00, 0x00,
|
||||
0x98, 0x40, 0x3f, 0x0c, 0x00, 0x00, 0x01, 0x0c, 0xc0, 0x36, 0x04, 0x00, 0x00, 0x01, 0x07, 0xe0,
|
||||
0x22, 0x04, 0x00, 0x00, 0x01, 0x07, 0x20, 0x22, 0x06, 0x00, 0x00, 0x02, 0x04, 0x20, 0x62, 0x02,
|
||||
0x00, 0x00, 0x02, 0x04, 0x20, 0x44, 0x02, 0x00, 0x00, 0x06, 0x02, 0x30, 0x44, 0x03, 0x00, 0x00,
|
||||
0x04, 0x02, 0x10, 0x44, 0x01, 0x00, 0x60, 0x04, 0x02, 0x30, 0xc4, 0x01, 0x80, 0x60, 0x08, 0x03,
|
||||
0xf0, 0xc8, 0x01, 0x80, 0x00, 0x18, 0x03, 0xb0, 0xf8, 0x00, 0xff, 0xff, 0xf0, 0x01, 0x20, 0x98,
|
||||
0x00, 0x7f, 0xff, 0xe0, 0x01, 0xe0, 0x98, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0xf0, 0x00, 0x00, 0x00, 0x07, 0xff, 0xff, 0xfc, 0x00, 0x00
|
||||
};
|
||||
|
||||
constexpr const unsigned char poivron_robotique [] PROGMEM = {
|
||||
0x00, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x60, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x02, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
|
||||
0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x82, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x42, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x21, 0x00, 0x00, 0x00, 0x01, 0xfe, 0x00, 0x21, 0x00, 0x00, 0x00, 0x06, 0x19,
|
||||
0x80, 0x21, 0x00, 0x00, 0x00, 0x08, 0x06, 0x60, 0x42, 0x00, 0x00, 0x00, 0x10, 0x01, 0x10, 0x42,
|
||||
0x00, 0x00, 0x00, 0x20, 0x00, 0x88, 0x42, 0x00, 0x00, 0x00, 0x20, 0x00, 0x44, 0x82, 0x00, 0x00,
|
||||
0x00, 0x40, 0x00, 0x42, 0x84, 0x00, 0x00, 0x00, 0x80, 0x01, 0xfb, 0x04, 0x00, 0x00, 0x00, 0x80,
|
||||
0x02, 0x06, 0x04, 0x00, 0x00, 0x01, 0x00, 0x00, 0x01, 0x86, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00,
|
||||
0x49, 0x80, 0x00, 0x02, 0x00, 0x00, 0x00, 0x28, 0x60, 0x00, 0x02, 0x00, 0x00, 0x00, 0x18, 0x10,
|
||||
0x00, 0x04, 0x00, 0x00, 0x00, 0x07, 0x08, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x04,
|
||||
0x00, 0x80, 0x00, 0x00, 0x24, 0x00, 0x08, 0x01, 0x00, 0x00, 0x00, 0x13, 0x00, 0x08, 0x01, 0x00,
|
||||
0x00, 0x00, 0x08, 0xc0, 0x10, 0x02, 0x00, 0x00, 0x00, 0x08, 0x20, 0x10, 0x02, 0x00, 0x00, 0x10,
|
||||
0x04, 0x20, 0x10, 0x04, 0x00, 0x00, 0x10, 0x04, 0x10, 0x20, 0x04, 0x00, 0x00, 0x10, 0x00, 0x10,
|
||||
0x20, 0x08, 0x00, 0x00, 0x20, 0x00, 0x10, 0x20, 0x08, 0x00, 0x00, 0x20, 0x00, 0x10, 0x20, 0x08,
|
||||
0x00, 0x00, 0x40, 0x00, 0x10, 0x40, 0x10, 0x00, 0x00, 0x48, 0x00, 0x10, 0x40, 0x10, 0x00, 0x00,
|
||||
0x8c, 0x00, 0x10, 0x40, 0x10, 0x00, 0x00, 0x9e, 0x01, 0x20, 0x40, 0x20, 0x00, 0x01, 0x1e, 0x03,
|
||||
0x20, 0x40, 0x20, 0x00, 0x01, 0x1f, 0x07, 0x40, 0x80, 0x20, 0x00, 0x02, 0x0c, 0x0e, 0x40, 0x80,
|
||||
0x40, 0x00, 0x02, 0x00, 0x00, 0x80, 0x80, 0x40, 0x00, 0x04, 0x00, 0x01, 0x00, 0x80, 0x40, 0x00,
|
||||
0x04, 0x00, 0x01, 0x00, 0x80, 0x80, 0x00, 0x08, 0x00, 0x02, 0x00, 0x80, 0x80, 0x00, 0x08, 0x3f,
|
||||
0x84, 0x00, 0x80, 0x80, 0x00, 0x10, 0xdf, 0xc4, 0x00, 0x80, 0x80, 0x00, 0x11, 0xdf, 0xe8, 0x00,
|
||||
0x81, 0x00, 0x00, 0x27, 0xff, 0xb0, 0x00, 0x81, 0x00, 0x00, 0x2f, 0xff, 0xb0, 0x00, 0x81, 0x00,
|
||||
0x00, 0x5f, 0xff, 0xe0, 0x00, 0x42, 0x00, 0x00, 0x5f, 0xff, 0xc0, 0x00, 0x42, 0x00, 0x00, 0xbf,
|
||||
0xff, 0xc0, 0x00, 0x42, 0x00, 0x00, 0xbf, 0xff, 0x80, 0x00, 0x22, 0x00, 0x01, 0x3f, 0xff, 0x00,
|
||||
0x00, 0x12, 0x00, 0x01, 0x3f, 0xfe, 0x00, 0x00, 0x0e, 0x00, 0x02, 0x1f, 0xfc, 0x00, 0x00, 0x02,
|
||||
0x00, 0x04, 0x1f, 0xf8, 0x00, 0x00, 0x01, 0x00, 0x08, 0x0f, 0xf0, 0x00, 0x00, 0x00, 0x80, 0x10,
|
||||
0x01, 0xe0, 0x00, 0x00, 0x00, 0x40, 0x20, 0x00, 0x80, 0x00, 0x00, 0x00, 0x3f, 0xc0, 0x03, 0x00,
|
||||
0x00, 0x00, 0x00, 0x00, 0x30, 0x1c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xe0, 0x00, 0x00, 0x00
|
||||
};
|
||||
}
|
||||
159
Main.ino
159
Main.ino
|
|
@ -1,14 +1,12 @@
|
|||
#include <stdexcept>
|
||||
#include <CodeCell.h>
|
||||
|
||||
#include "IO.hpp"
|
||||
#include "Images.hpp"
|
||||
|
||||
#define UNIT_TESTS
|
||||
|
||||
/*
|
||||
Etapes :
|
||||
- 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
|
||||
- 2eme etape : avncer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
|
||||
- 2eme etape : avancer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
|
||||
- 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
|
||||
- 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
|
||||
- 5eme etape : faire tourner actionneurs pour figurine
|
||||
|
|
@ -35,32 +33,55 @@ enum class State {
|
|||
|
||||
// CodeCell implementation
|
||||
CodeCell code_cell;
|
||||
IO my_IO {};
|
||||
IO my_IO {&code_cell};
|
||||
|
||||
void setup()
|
||||
{
|
||||
// Initialise serial
|
||||
Serial.begin(115200);
|
||||
|
||||
Serial.println("Starting Initialisation");
|
||||
|
||||
// Initialise the CodeCell for angle and tap detectIOn
|
||||
code_cell.Init(MOTION_GYRO + MOTION_TAP_DETECTOR + MOTION_ACCELEROMETER);
|
||||
code_cell.Init(MOTION_ROTATION);
|
||||
|
||||
Serial.println("CACA");
|
||||
// Initialise IO
|
||||
int nb_errors = my_IO.init();
|
||||
|
||||
// Initialise
|
||||
my_IO.init();
|
||||
if(nb_errors)
|
||||
{
|
||||
Serial.printf("%d errors occured during IO init.", nb_errors);
|
||||
while(true);
|
||||
}
|
||||
|
||||
my_IO.getScreen()->drawBitmap(0, 0, Imgs::riombotique, 52, 64, WHITE);
|
||||
my_IO.getScreen()->drawBitmap(75, 0, Imgs::poivron_robotique, 52, 64, WHITE);
|
||||
|
||||
//my_IO.motorControlOn();
|
||||
|
||||
Serial.println("Initalisation Finished");
|
||||
|
||||
#ifdef UNIT_TESTS
|
||||
Serial.println("UNIT_TESTS");
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef UNIT_TESTS
|
||||
|
||||
void loop()
|
||||
{
|
||||
if(code_cell.Run())
|
||||
{
|
||||
my_IO.update();
|
||||
my_IO.update();
|
||||
|
||||
my_IO.isTirettePulled();
|
||||
}
|
||||
Serial.print("Is Tirette Pulled : ");
|
||||
Serial.println(my_IO.isTirettePulled());
|
||||
|
||||
Serial.print("Is Color Blue : ");
|
||||
Serial.println(my_IO.isColorBlue());
|
||||
|
||||
my_IO.m_motors[0].setSpeed(3200);
|
||||
my_IO.m_motors[1].setSpeed(0);
|
||||
//my_IO.setDir(0);
|
||||
//my_IO.setSpeed(1024.0f);
|
||||
}
|
||||
|
||||
#else
|
||||
|
|
@ -69,76 +90,72 @@ void loop()
|
|||
{
|
||||
static State actual_state {State::ForwardToRamp};
|
||||
|
||||
// Runs every 100ms - Put your code here
|
||||
if(code_cell.Run())
|
||||
switch(actual_state)
|
||||
{
|
||||
switch(actual_state)
|
||||
{
|
||||
case State::WaitingForTirette:
|
||||
if(IO::ext_controls.isTirettePulled())
|
||||
{
|
||||
actual_state = State::WaitingTimer;
|
||||
}
|
||||
break;
|
||||
case State::WaitingForTirette:
|
||||
if(my_IO.isTirettePulled())
|
||||
{
|
||||
actual_state = State::WaitingTimer;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::WaitingTimer:
|
||||
static unsigned long initial_time = millis();
|
||||
case State::WaitingTimer:
|
||||
static unsigned long initial_time = millis();
|
||||
|
||||
if(millis() - initial_time >= 90000)
|
||||
{
|
||||
actual_state = State::ForwardToRamp;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::ForwardToRamp:
|
||||
IO::motors.forward();
|
||||
if(millis() - initial_time >= 90000)
|
||||
{
|
||||
actual_state = State::ForwardToRamp;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::ForwardToRamp:
|
||||
my_IO.forward();
|
||||
|
||||
if(IO::gyro.getAngle() > 9)
|
||||
{
|
||||
actual_state = State::ForwardToScene;
|
||||
}
|
||||
break;
|
||||
if(my_IO.getAngle() > 9)
|
||||
{
|
||||
actual_state = State::ForwardToScene;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::ForwardToScene:
|
||||
IO::motors.forward();
|
||||
case State::ForwardToScene:
|
||||
IO::motors.forward();
|
||||
|
||||
if(IO::gyro.getAngle() < 1)
|
||||
{
|
||||
actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
|
||||
}
|
||||
break;
|
||||
if(IO::gyro.getAngle() < 1)
|
||||
{
|
||||
actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::Turn90Blue:
|
||||
IO::motors.turnLeft();
|
||||
case State::Turn90Blue:
|
||||
IO::motors.turnLeft();
|
||||
|
||||
if(IO::gyro.getAngle() > 90)
|
||||
{
|
||||
actual_state = State::ForwardToSceneEdge;
|
||||
}
|
||||
break;
|
||||
if(IO::gyro.getAngle() > 90)
|
||||
{
|
||||
actual_state = State::ForwardToSceneEdge;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::Turn90Yellow:
|
||||
IO::motors.turnRight();
|
||||
case State::Turn90Yellow:
|
||||
IO::motors.turnRight();
|
||||
|
||||
if(IO::gyro.getAngle() > 90)
|
||||
{
|
||||
actual_state = State::ForwardToSceneEdge;
|
||||
}
|
||||
break;
|
||||
if(IO::gyro.getAngle() > 90)
|
||||
{
|
||||
actual_state = State::ForwardToSceneEdge;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::ForwardToSceneEdge:
|
||||
IO::motors.forward();
|
||||
case State::ForwardToSceneEdge:
|
||||
IO::motors.forward();
|
||||
|
||||
if(IO::gyro.tapDetected())
|
||||
{
|
||||
actual_state = State::Dancing;
|
||||
}
|
||||
break;
|
||||
if(IO::gyro.getAngle() < 0)
|
||||
{
|
||||
actual_state = State::Dancing;
|
||||
}
|
||||
break;
|
||||
|
||||
case State::Dancing:
|
||||
IO::ext_controls.startSpecialAction();
|
||||
break;
|
||||
}
|
||||
case State::Dancing:
|
||||
IO::ext_controls.startSpecialAction();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue