26 lines
492 B
C++
26 lines
492 B
C++
|
/*
|
||
|
|
* movement.cpp
|
||
|
|
*
|
||
|
|
* Created on: 17.01.2021
|
||
|
|
* Author: frank
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "Arduino.h"
|
||
|
|
#include "movement.hpp"
|
||
|
|
|
||
|
|
int angle = 0;
|
||
|
|
int motor_position;
|
||
|
|
|
||
|
|
void slither(){
|
||
|
|
float radiant = (angle % 360) * DEG_TO_RAD;
|
||
|
|
for(int servo_id = SERVO_COUNT; servo_id > 0; servo_id--) {
|
||
|
|
motor_position = AMPLITUDE * sin(radiant + servo_id * PHASE_SHIFT * WAVE_LENGTH);
|
||
|
|
map_angle_to_servo_with_initial_position(servo_id, motor_position);
|
||
|
|
delay(INTER_SERVO_MOVEMENT_DELAY_MS);
|
||
|
|
}
|
||
|
|
angle++;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|