/*
 * 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++;
}