New Code / Cleanup
This commit is contained in:
Lars H
parent
3dbc6cf151
commit
34ca1c5e01
116
Zowi.cpp
116
Zowi.cpp
@ -1,116 +0,0 @@
|
|||||||
#include "Zowi.h"
|
|
||||||
#include <Oscillator.h>
|
|
||||||
|
|
||||||
void Zowi::init(int YL, int YR, int RL, int RR, bool load_calibration) {
|
|
||||||
servo[0].attach(YL);
|
|
||||||
servo[1].attach(YR);
|
|
||||||
servo[2].attach(RL);
|
|
||||||
servo[3].attach(RR);
|
|
||||||
|
|
||||||
if (load_calibration) {
|
|
||||||
for (int i = 0; i < 4; i++) {
|
|
||||||
int servo_trim = EEPROM.read(i);
|
|
||||||
if (servo_trim > 128) servo_trim -= 256;
|
|
||||||
servo[i].SetTrim(servo_trim);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Serial.begin(115200);
|
|
||||||
Serial.println(" test");
|
|
||||||
|
|
||||||
for (int i = 0; i < 4; i++) servo_position[i] = 90;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::setTrims(int YL, int YR, int RL, int RR) {
|
|
||||||
servo[0].SetTrim(YL);
|
|
||||||
servo[1].SetTrim(YR);
|
|
||||||
servo[2].SetTrim(RL);
|
|
||||||
servo[3].SetTrim(RR);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::saveTrimsOnEEPROM() {
|
|
||||||
for (int i = 0; i < 4; i++) EEPROM.write(i, servo[i].getTrim());
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::moveServos(int time, int servo_target[]) {
|
|
||||||
if(time>10){
|
|
||||||
for (int i = 0; i < 4; i++) increment[i] = ((servo_target[i]) - servo_position[i]) / (time / 10.0);
|
|
||||||
final_time = millis() + time;
|
|
||||||
|
|
||||||
for (int iteration = 1; millis() < final_time; iteration++) {
|
|
||||||
partial_time = millis() + 10;
|
|
||||||
for (int i = 0; i < 4; i++) servo[i].SetPosition(servo_position[i] + (iteration * increment[i]));
|
|
||||||
while (millis() < partial_time); //pause
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
for (int i = 0; i < 4; i++) servo[i].SetPosition(servo_target[i]);
|
|
||||||
}
|
|
||||||
for (int i = 0; i < 4; i++) servo_position[i] = servo_target[i];
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::oscillateServos(int A[4], int O[4], int T, double phase_diff[4], float cycle=1){
|
|
||||||
for (int i=0; i<4; i++) {
|
|
||||||
servo[i].SetO(O[i]);
|
|
||||||
servo[i].SetA(A[i]);
|
|
||||||
servo[i].SetT(T);
|
|
||||||
servo[i].SetPh(phase_diff[i]);
|
|
||||||
}
|
|
||||||
double ref=millis();
|
|
||||||
for (double x=ref; x<T*cycle+ref; x=millis()){
|
|
||||||
for (int i=0; i<4; i++){
|
|
||||||
servo[i].refresh();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
void Zowi::walk(float steps, int T){
|
|
||||||
int A[4]= {30, 30, 20, 20};
|
|
||||||
int O[4] = {0, 0, 0, 0};
|
|
||||||
double phase_diff[4] = {DEG2RAD(180), DEG2RAD(180), DEG2RAD(90), DEG2RAD(90)};
|
|
||||||
|
|
||||||
int cycles=(int)steps;
|
|
||||||
|
|
||||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
|
||||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::backward(float steps, int T){
|
|
||||||
int A[4]= {15, 15, 30, 30};
|
|
||||||
int O[4] = {0, 0, 0, 0};
|
|
||||||
double phase_diff[4] = {DEG2RAD(180), DEG2RAD(180), DEG2RAD(270), DEG2RAD(270)};
|
|
||||||
|
|
||||||
int cycles=(int)steps;
|
|
||||||
|
|
||||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
|
||||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::turnLeft(float steps, int T){
|
|
||||||
int A[4]= {10, 10, 25, 25};
|
|
||||||
int O[4] = {0, 0, 0, 0};
|
|
||||||
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(180), DEG2RAD(90), DEG2RAD(90)};
|
|
||||||
|
|
||||||
int cycles=(int)steps;
|
|
||||||
|
|
||||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
|
||||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::turnRight(float steps, int T){
|
|
||||||
int A[4]= {20, 20, 30, 10};
|
|
||||||
int O[4] = {0, 0, 0, 0};
|
|
||||||
double phase_diff[4] = {DEG2RAD(0), DEG2RAD(0), DEG2RAD(90), DEG2RAD(90)};
|
|
||||||
|
|
||||||
int cycles=(int)steps;
|
|
||||||
|
|
||||||
if (cycles >= 1) for(int i=0;i<cycles;i++) oscillateServos(A,O, T, phase_diff);
|
|
||||||
oscillateServos(A,O, T, phase_diff,(float)steps-cycles);
|
|
||||||
}
|
|
||||||
|
|
||||||
void Zowi::jump(float steps, int T){
|
|
||||||
int up[]={90,90,150,30};
|
|
||||||
moveServos(T,up);
|
|
||||||
int down[]={90,90,90,90};
|
|
||||||
moveServos(T,down);
|
|
||||||
}
|
|
||||||
42
Zowi.h
42
Zowi.h
@ -1,42 +0,0 @@
|
|||||||
#ifndef Zowi_h
|
|
||||||
#define Zowi_h
|
|
||||||
|
|
||||||
|
|
||||||
#include "Arduino.h"
|
|
||||||
#include <ESP32_Servo.h>
|
|
||||||
#include <Oscillator.h>
|
|
||||||
#include <EEPROM.h>
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
class Zowi
|
|
||||||
{
|
|
||||||
public:
|
|
||||||
void init(int YL, int YR, int RL, int RR, bool load_calibration=0);
|
|
||||||
void setTrims(int YL, int YR, int RL, int RR);
|
|
||||||
void saveTrimsOnEEPROM();
|
|
||||||
|
|
||||||
void moveServos(int time, int servo_target[]);
|
|
||||||
void oscillateServos(int A[4], int O[4], int T, double phase_diff[4], float cycle);
|
|
||||||
|
|
||||||
void walk(float steps=4, int T=1566);
|
|
||||||
void turnLeft(float steps, int T);
|
|
||||||
void turnRight(float steps, int T);
|
|
||||||
void backward(float steps, int T);
|
|
||||||
void jump(float steps=1, int T=1000);
|
|
||||||
|
|
||||||
|
|
||||||
private:
|
|
||||||
|
|
||||||
Oscillator servo[4];
|
|
||||||
int servo_trim[4];
|
|
||||||
int servo_position[4];
|
|
||||||
|
|
||||||
unsigned long final_time;
|
|
||||||
unsigned long partial_time;
|
|
||||||
float increment[4];
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user