#include "motor.h"
#include "oled.h"
#include "list_motor.h"

motor_t gm6020;
motor_t m3508_frontleft;
motor_t m3508_frontright;
motor_t m3508_backleft;
motor_t m3508_backright;
motor_t m2006;

const pid_struct_t M3508_pid_speed = { //A VERIFIER
		1, 2, 0.005, 2, 16000, 16000
	};
const pid_struct_t M3508_pid_pos = { //A VERIFIER
		0, 0.1, 0.005, 0, 16000, 16000
	};

const pid_struct_t GM6020_pid_speed = { //A VERIFIER
		1, 40, 10, 0, 30000, 30000
	};
const pid_struct_t GM6020_pid_pos = { //A VERIFIER
		0, 12, 3, 5, 10000, 10000
	};

const pid_struct_t M2006_pid_speed = { //A FAIRE
		1, 1.5, 0.1, 0, 16384, 16384
	};

const pid_struct_t M2006_pid_pos = { //A FAIRE
		0, 12, 3, 5, 10000, 10000
	};

void motors_init(void){
	gm6020.type = GM6020;
	gm6020.can_info.can_id = 0;
	gm6020.can_info.tx_id = 1;
	gm6020.can_info.rx_id = 0x205; 
	gm6020.pid_pos = GM6020_pid_pos;
	gm6020.pid_speed = GM6020_pid_speed;
	
	m3508_frontleft.type = M3508;
	m3508_frontleft.can_info.can_id = 0;
	m3508_frontleft.can_info.tx_id = 0;
	m3508_frontleft.can_info.rx_id = 0x201; 
	m3508_frontleft.pid_pos = M3508_pid_pos;
	m3508_frontleft.pid_speed = M3508_pid_speed;
	
	m3508_frontright.type = M3508;
	m3508_frontright.can_info.can_id = 0;
	m3508_frontright.can_info.tx_id = 0;
	m3508_frontright.can_info.rx_id = 0x202; 
	m3508_frontright.pid_pos = M3508_pid_pos;
	m3508_frontright.pid_speed = M3508_pid_speed;
	
	m3508_backleft.type = M3508;
	m3508_backleft.can_info.can_id = 0;
	m3508_backleft.can_info.tx_id = 0;
	m3508_backleft.can_info.rx_id = 0x203; 
	m3508_backleft.pid_pos = M3508_pid_pos;
	m3508_backleft.pid_speed = M3508_pid_speed;
	
	m3508_backright.type = M3508;
	m3508_backright.can_info.can_id = 0;
	m3508_backright.can_info.tx_id = 0;
	m3508_backright.can_info.rx_id = 0x204; 
	m3508_backright.pid_pos = M3508_pid_pos;
	m3508_backright.pid_speed = M3508_pid_speed;
	
	m2006.type = M2006;
	m2006.can_info.can_id = 0;
	m2006.can_info.tx_id = 1;
	m2006.can_info.rx_id = 0x207; 
	m2006.pid_pos = M2006_pid_pos;
	m2006.pid_speed = M2006_pid_speed;
}