WunderSpider

  /*******************************************************
*  --- Hexapod Spider A.K.A. "Roberto II" ---
*  Main Application. Runs in a nRF51822 (cortex M0 + BLE)
*  Author: Daniel Mancuso.
*******************************************************/
#include "nrf_pwm.h"

//Hardware pinout definitions:
#define PWM_CH0 15	//Front Legs
#define PWM_CH1 16	//Side Legs (2 servos)

#define PWM_TIMER_INTERVAL  20	//tick interval (check real lenght)

#define PWM_MIN 132 // 1ms   --> -45 degrees   
#define PWM_CENTER  PWM_MIN + (PWM_MAX-PWM_MIN)/2 // 1.5ms -->   0 degrees
#define PWM_MAX 264 // 2ms   --> +45 degrees

#define FRONT_LEGS_LIMIT  37
#define SIDE_LEGS_LIMIT 17
#define MOVING_SPEED  9

//BLE Control Commands for the Hexapod:
#define STOP  0
#define MOVE_FORWARD  1
#define MOVE_BACKWARD 2
#define MOVE_LEFT 3
#define MOVE_RIGHT  4
#define SHAKE 5

sensor_bridge_t sensor_bridge;

struct channel{
	uint16_t actual_position;
	uint16_t final_position;	//should be simetrical 
	uint16_t max_position;
	uint16_t min_position;	
	uint16_t center_position;
	uint8_t direction;
	uint8_t speed;
};

typedef struct{
	uint8_t movement_cmd;
	uint8_t current_frame;
	uint8_t frame_end_flag;
	uint8_t frame_length;
	uint16_t i;
	struct channel ch0;
	struct channel ch1;
}hexapod_struct;

static hexapod_struct hexapod;

// ---------------------------------------
// --- Initialization ---
// ---------------------------------------
void pwm_init(void){

	  nrf_pwm_config_t pwm_config = PWM_DEFAULT_CONFIG;
    
    pwm_config.mode             = PWM_MODE_SERVOS;
    pwm_config.num_channels     = 2;
    pwm_config.gpio_num[0]      = PWM_CH0;
    pwm_config.gpio_num[1]      = PWM_CH1;
    //pwm_config.gpio_num[2]      = 10;    
    
    // Initialize the PWM library
    nrf_pwm_init(&pwm_config);
		
		hexapod.current_frame = 0;
		hexapod.frame_end_flag = 0;
		hexapod.movement_cmd = 0;

		hexapod.ch0.speed = MOVING_SPEED;//5;	//here we can adjust speed of center legs servo
		hexapod.ch0.min_position = PWM_MIN + FRONT_LEGS_LIMIT;
		hexapod.ch0.max_position = PWM_MAX - FRONT_LEGS_LIMIT;
	
		hexapod.ch1.speed = MOVING_SPEED;//14;	//here we can adjust speed of side legs servos
		hexapod.ch1.min_position = PWM_MIN + SIDE_LEGS_LIMIT;
		hexapod.ch1.max_position = PWM_MAX - SIDE_LEGS_LIMIT;
		
		hexapod.ch0.center_position = hexapod.ch0.min_position + (hexapod.ch0.max_position-hexapod.ch0.min_position)/2;
		hexapod.ch1.center_position = hexapod.ch1.min_position + (hexapod.ch1.max_position-hexapod.ch1.min_position)/2;

		hexapod.frame_length = (hexapod.ch1.max_position - hexapod.ch1.min_position) / hexapod.ch1.speed;

		hexapod.ch0.actual_position = hexapod.ch0.center_position;
		hexapod.ch1.actual_position = hexapod.ch1.center_position;
		
		nrf_pwm_set_value(0, hexapod.ch0.actual_position);
		nrf_pwm_set_value(1, hexapod.ch1.actual_position);
}

/**
**	uint8_t Update_servos(void)
**
**  Checks if actual_position is inside the limits and set the PWM value.
**	input: none
**	output: 1 on Excedded limit
**          0 OK	
**/
uint8_t Update_servos(void)
{		
		if ((hexapod.ch0.actual_position > hexapod.ch0.max_position)
				|| (hexapod.ch1.actual_position > hexapod.ch1.max_position)
				|| (hexapod.ch0.actual_position  min
	if ((frames[current_f][0] == -1)	&& 
			hexapod.ch0.actual_position >= (hexapod.ch0.min_position + hexapod.ch0.speed))
				hexapod.ch0.actual_position -= hexapod.ch0.speed;
	//ch0 aproaching center from rigth
	if ((frames[current_f][0] == 0)	&& 
			hexapod.ch0.actual_position >= (hexapod.ch0.center_position + hexapod.ch0.speed))
				hexapod.ch0.actual_position -= hexapod.ch0.speed;
	//ch0 aproaching center from left
	if ((frames[current_f][0] == 0)	&& 
			hexapod.ch0.actual_position  max
	if ((frames[current_f][0] == 1)	&& 
			hexapod.ch0.actual_position  min
	if ((frames[current_f][1] == -1)	&& 
			hexapod.ch1.actual_position >= (hexapod.ch1.min_position + hexapod.ch1.speed))
				hexapod.ch1.actual_position -= hexapod.ch1.speed;
	//ch1 aproaching center from rigth
	if ((frames[current_f][1] == 0)	&& 
			hexapod.ch1.actual_position >= (hexapod.ch1.center_position + hexapod.ch1.speed))
				hexapod.ch1.actual_position -= hexapod.ch1.speed;
	//ch1 aproaching center from left
	if ((frames[current_f][1] == 0)	&& 
			hexapod.ch1.actual_position  max
	if ((frames[current_f][1] == 1)	&& 
			hexapod.ch1.actual_position  positive angles (move right), 0 -> negative (left)
	//Frame:   Central Legs angle(CH0):   Front/Back Legs angle(CH1):	
	//     0	  	     0									0
	//     1				 +45								+45	
	//     2				 +45								-45
	//     3				 -45								-45
	//     4				 -45								+45

	//note: CenterLegs positive -> anti-clockwise
	//      Side legs positive -> clockwise
	// +1: Max_position
	// -1: Min_position
	//  0: Center_position
	// i: Frame number, j: channel number
	const int8_t forward[4][2]  = {{-1,1},{-1,-1},{ 1,-1},{ 1,1}};//{{1,-1},{-1,-1},{ -1,1},{ 1,1}};
	const int8_t backward[4][2] = {{ 1,1},{ 1,-1},{-1,-1},{-1,1}};
	
	const int8_t left[4][2]    = {{-1,-1},{ 0, 1},{-1,-1},{0,1}};
	const int8_t right[4][2]	 = {{ 1, 1},{ 0,-1},{1, 1},{ 0,-1}};	//trying turns limiting position of center legs (CH0)	
	
	
	int16_t shake[] = {1, 10, 20, 28, 20, 10, 0, -10, -20, -5, 10, -15, -5, -12, -7};
	static uint8_t  shake_i = 0;
	
	
	
	switch (hexapod.movement_cmd){
		case MOVE_FORWARD:
			update_channels_end(forward, hexapod.current_frame);
			break;
		case MOVE_BACKWARD:
			update_channels_end(backward, hexapod.current_frame);
			break;
		case MOVE_LEFT:
			update_channels_end(left, hexapod.current_frame);
			break;
		case MOVE_RIGHT:
			update_channels_end(right, hexapod.current_frame);
			break;
		case SHAKE:
			if (shake_i++ >= (uint8_t)sizeof(shake))	shake_i=0;
			hexapod.ch0.actual_position = hexapod.ch0.center_position + shake[shake_i];
			hexapod.ch1.actual_position = hexapod.ch1.center_position + shake[shake_i];
			Update_servos();
			break;
	}
	
	if (hexapod.i++ >= hexapod.frame_length){
		hexapod.i = 0;
		if (hexapod.current_frame++ >= 3)
			hexapod.current_frame = 0;
	}	
}

/////////////////////////////////////////////////////////////////////////
/**@brief      Application tick handler.
 *
 * @param[in]   ctx		Value that will be passed back to the callback  
 *
 * @retval      None.
 */
void app_tick_handler(void * ctx)  //void pwm_tick_handler(void * ctx) 
{
	update_hexapod(ctx);
}

/////////////////////////////////////////////////////////////////////////
void my_characteristic_write_callback(ble_characteristic_info_t* char_info, uint16_t offset, uint16_t len, uint8_t* data) 
{	
		uint32_t pwm_value;
		
		if ((char_info == &characteristic_sensorData_w_info) && (offset == 0) && (len == sizeof(sensor_bridge.data_w.packet))) 
		{
				memcpy((uint8_t*)&sensor_bridge.data_w, data, len);	
			
			  	if (sensor_bridge.data_w.packet[0] 
						hexapod.movement_cmd = sensor_bridge.data_w.packet[0];
						hexapod.current_frame = 0;
						hexapod.frame_end_flag = 0;
				}
				if (sensor_bridge.data_w.packet[1]