diff --git a/Hippie.cpp b/Hippie.cpp index 919bea1..619fb7a 100644 --- a/Hippie.cpp +++ b/Hippie.cpp @@ -6,7 +6,7 @@ void Hippie::init(int YL, int YR, int RL, int RR, int Buzzer) { - + servo_pins[0] = YL; servo_pins[1] = YR; servo_pins[2] = RL; @@ -15,11 +15,11 @@ void Hippie::init(int YL, int YR, int RL, int RR, int Buzzer) { attachServos(); isHippieResting=false; - + for (int i = 0; i < 4; i++) servo_position[i] = 90; - //Buzzer & noise sensor pins: + //Buzzer & noise sensor pins: pinBuzzer = Buzzer; pinMode(Buzzer,OUTPUT); @@ -95,14 +95,14 @@ void Hippie::_execute(int A[4], int O[4], int T, double phase_diff[4], float ste } - int cycles=(int)steps; + int cycles=(int)steps; //-- Execute complete cycles - if (cycles >= 1) - for(int i = 0; i < cycles; i++) + if (cycles >= 1) + for(int i = 0; i < cycles; i++) oscillateServos(A,O, T, phase_diff); - - //-- Execute the final not complete cycle + + //-- Execute the final not complete cycle oscillateServos(A,O, T, phase_diff,(float)steps-cycles); } @@ -156,15 +156,15 @@ void Hippie::jump(float steps, int T){ //-- Hippie Test Positions (bring feets and hips in certain position) //--------------------------------------------------------- void Hippie::test_pos(){ - + int left_feet_up[4]={90,0,90,30}; //watch from view of robot: [3] = left leg ... by + value: turn right _moveServos(1000,left_feet_up); // [4] = right leg ... by + value: right side up - + } //--------------------------------------------------------- -//-- Hippie gait: Walking (forward or backward) +//-- Hippie gait: Walking (forward or backward) //-- Parameters: //-- * steps: Number of steps //-- * T : Period @@ -182,11 +182,11 @@ void Hippie::walk(float steps, int T, int dir){ int A[4]= {30, 30, 40, 40}; //20 int O[4] = {0, 0, 4, 30}; //-4 double phase_diff[4] = {0, 0, DEG2RAD(dir * -90), DEG2RAD(dir * -90)}; - + if ( dir == -1) { double phase_diff[4] = {0, 0, DEG2RAD(dir * 90), DEG2RAD(dir * 90)}; } //-- Let's oscillate the servos! - _execute(A, O, T, phase_diff, steps); + _execute(A, O, T, phase_diff, steps); } @@ -202,13 +202,13 @@ void Hippie::turn(float steps, int T, int dir){ //-- Same coordination than for walking (see Hippie::walk) //-- The Amplitudes of the hip's oscillators are not igual //-- When the right hip servo amplitude is higher, the steps taken by - //-- the right leg are bigger than the left. So, the robot describes an + //-- the right leg are bigger than the left. So, the robot describes an //-- left arc int A[4]= {30, 30, 20, 20}; int O[4] = {0, 0, 4, 30}; - double phase_diff[4] = {0, 0, DEG2RAD(-90), DEG2RAD(-90)}; - - if (dir == LEFT) { + double phase_diff[4] = {0, 0, DEG2RAD(-90), DEG2RAD(-90)}; + + if (dir == LEFT) { A[0] = 50; //-- Left hip servo A[1] = 10; //-- Right hip servo } @@ -217,9 +217,9 @@ void Hippie::turn(float steps, int T, int dir){ A[1] = 50; A[2] = 40; } - + //-- Let's oscillate the servos! - _execute(A, O, T, phase_diff, steps); + _execute(A, O, T, phase_diff, steps); } @@ -233,14 +233,14 @@ void Hippie::turn(float steps, int T, int dir){ void Hippie::bend (int steps, int T, int dir){ //Parameters of all the movements. Default: Left bend - int bend1[4]={90, 90, 62, 35}; + int bend1[4]={90, 90, 62, 35}; int bend2[4]={90, 90, 62, 105+60}; int homes[4]={90, 90, 90, 90}; //Time of one bend, constrained in order to avoid movements too fast. //T=max(T, 600); - //Changes in the parameters if right direction is chosen + //Changes in the parameters if right direction is chosen if(dir==-1) { bend1[2]=180-35; @@ -250,7 +250,7 @@ void Hippie::bend (int steps, int T, int dir){ } //Time of the bend movement. Fixed parameter to avoid falls - int T2=800; + int T2=800; //Bend movement for (int i=0;i #include -#include "Hippie_mouths.h" -#include "Hippie_sounds.h" -#include "Hippie_gestures.h" - - //-- Constants #define FORWARD 1 #define BACKWARD -1 @@ -18,11 +12,6 @@ #define MEDIUM 15 #define BIG 30 -#define PIN_Buzzer 10 -#define PIN_Trigger 8 -#define PIN_Echo 9 -#define PIN_NoiseSensor A6 - class Hippie { @@ -43,7 +32,7 @@ class Hippie void home(); bool getRestState(); void setRestState(bool state); - + //-- Predetermined Motion Functions void jump(float steps=1, int T = 600); @@ -61,15 +50,15 @@ class Hippie void moonwalker(float steps=1, int T=900, int h=50, int dir=LEFT); void crusaito(float steps=1, int T=900, int h=20, int dir=FORWARD); void flapping(float steps=1, int T=1000, int h=50, int dir=FORWARD); - - void test_pos(); + + void test_pos(); void new_walk(int dir = FORWARD, float steps =4, int T=750); - void new_turn(int dir = LEFT, float steps =2, int T=1000); + void new_turn(int dir = LEFT, float steps =2, int T=1000); + - private: - + Oscillator servo[4]; int servo_pins[4]; @@ -77,7 +66,7 @@ class Hippie int servo_position[4]; int pinBuzzer; - + unsigned long final_time; unsigned long partial_time; float increment[4]; @@ -89,5 +78,3 @@ class Hippie }; #endif - - diff --git a/Oscillator.cpp b/Oscillator.cpp index 4d92273..a99da55 100644 --- a/Oscillator.cpp +++ b/Oscillator.cpp @@ -5,31 +5,31 @@ //-- (c) Juan Gonzalez-Gomez (Obijuan), Dec 2011 //-- GPL license //-------------------------------------------------------------- -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" - #include -#endif + +#include "Arduino.h" #include "Oscillator.h" -#include +#if defined(ESP32) + #include +#else + #include +#endif //-- This function returns true if another sample //-- should be taken (i.e. the TS time has passed since //-- the last sample was taken bool Oscillator::next_sample() { - + //-- Read current time _currentMillis = millis(); - + //-- Check if the timeout has passed if(_currentMillis - _previousMillis > _TS) { - _previousMillis = _currentMillis; + _previousMillis = _currentMillis; return true; } - + return false; } @@ -39,9 +39,6 @@ bool Oscillator::next_sample() void Oscillator::attach(int pin, bool rev) { //-- If the oscillator is detached, attach it. - //Serial.println("PANIIIIIIIIIIIIIIIIIC"); -// Serial.println(!_servo.attached()); - //if(!_servo.attached()){ //-- Attach the servo and move it to the home position _servo.attach(pin); @@ -64,8 +61,7 @@ void Oscillator::attach(int pin, bool rev) //-- Reverse mode _rev = rev; - //} - + } //-- Detach an oscillator from his servo @@ -84,7 +80,7 @@ void Oscillator::SetT(unsigned int T) { //-- Assign the new period _T=T; - + //-- Recalculate the parameters __N = _T/_TS; _inc = 2*M_PI/__N; @@ -107,10 +103,10 @@ void Oscillator::SetPosition(int position) /*******************************************************************/ void Oscillator::refresh() { - + //-- Only When TS milliseconds have passed, the new sample is obtained if (next_sample()) { - + //-- If the oscillator is not stopped, calculate the servo position if (!_stop) { //-- Sample the sine function and set the servo pos diff --git a/Oscillator.h b/Oscillator.h index 0fc924e..96ae443 100644 --- a/Oscillator.h +++ b/Oscillator.h @@ -8,7 +8,11 @@ #ifndef Oscillator_h #define Oscillator_h -#include +#if defined(ESP32) + #include +#else + #include +#endif //-- Macro for converting from degrees to radians #ifndef DEG2RAD @@ -21,32 +25,32 @@ class Oscillator Oscillator(int trim=0) {_trim=trim;}; void attach(int pin, bool rev =false); void detach(); - + void SetA(unsigned int A) {_A=A;}; void SetO(unsigned int O) {_O=O;}; void SetPh(double Ph) {_phase0=Ph;}; void SetT(unsigned int T); void SetTrim(int trim){_trim=trim;}; int getTrim() {return _trim;}; - void SetPosition(int position); + void SetPosition(int position); void Stop() {_stop=true;}; void Play() {_stop=false;}; void Reset() {_phase=0;}; void refresh(); - + private: - bool next_sample(); - + bool next_sample(); + private: //-- Servo that is attached to the oscillator Servo _servo; - + //-- Oscillators parameters unsigned int _A; //-- Amplitude (degrees) unsigned int _O; //-- Offset (degrees) unsigned int _T; //-- Period (miliseconds) double _phase0; //-- Phase (radians) - + //-- Internal variables int _pos; //-- Current servo pos int _trim; //-- Calibration offset @@ -54,10 +58,10 @@ class Oscillator double _inc; //-- Increment of phase double __N; //-- Number of samples unsigned int _TS; //-- sampling period (ms) - - long _previousMillis; + + long _previousMillis; long _currentMillis; - + //-- Oscillation mode. If true, the servo is stopped bool _stop; @@ -65,4 +69,4 @@ class Oscillator bool _rev; }; -#endif \ No newline at end of file +#endif