2025 Robot - Mecanum Wheel Drive Modes

#include "pico/stdlib.h"

enum Mecanum_Drive_Mode {
	STOP,
	FORWARD,
	REVERSE,
	ROTATE_CW,
	ROTATE_CCW,
	STRAFE_LEFT,
	STRAFE_RIGHT,
	DIAG_FL,
	DIAG_FR,
	DIAG_BL,
	DIAG_BR
}; 

enum Mecanum_Drive_Mode drive_mode = STOP;

const uint led_onb = 25;

const uint btn_mode = 26;

const uint motor_la_fwd = 2;
const uint motor_la_rev = 3;
const uint motor_lb_fwd = 4;
const uint motor_lb_rev = 5;

const uint motor_ra_fwd = 6;
const uint motor_ra_rev = 7;
const uint motor_rb_fwd = 8;
const uint motor_rb_rev = 9;

float fwd_speed_mm_per_sec = 390.0f;


uint on_delay = 3000;
uint off_delay = 100;

void setup_mode_btn();
void setup_motor_pins();
void test_all_h_bridge_pins();
void set_drive_mode(enum Mecanum_Drive_Mode mode);

int main() {
	// setup &  initialize
	setup_motor_pins();
	setup_mode_btn();
	
	gpio_init(led_onb);
	gpio_set_dir(led_onb, GPIO_OUT);

	// primary loop
		while (true) {
			if(gpio_get(btn_mode) == 0 ){
				set_drive_mode(STOP);
				sleep_ms(off_delay);

				drive_mode++;
				if(drive_mode > DIAG_BR) drive_mode = FORWARD;
				
				gpio_put(led_onb, 1);
				set_drive_mode(drive_mode);
				sleep_ms(on_delay);
				gpio_put(led_onb, 0);
				
				set_drive_mode(STOP);
			}

		}


}

void setup_motor_pins(){

	gpio_init(motor_la_fwd);
	gpio_set_dir(motor_la_fwd ,GPIO_OUT);
	gpio_put(motor_la_fwd, 0);

	gpio_init(motor_la_rev);
	gpio_set_dir( motor_la_rev,GPIO_OUT);
	gpio_put(motor_la_rev, 0);

	gpio_init(motor_lb_fwd);
	gpio_set_dir(motor_lb_fwd ,GPIO_OUT);
	gpio_put(motor_lb_fwd, 0);

	gpio_init(motor_lb_rev);
	gpio_set_dir(motor_lb_rev ,GPIO_OUT);
	gpio_put(motor_lb_rev, 0);

	///////////////////////////

	gpio_init(motor_ra_fwd);
	gpio_set_dir(motor_ra_fwd ,GPIO_OUT);
	gpio_put(motor_ra_fwd, 0);

	gpio_init(motor_ra_rev);
	gpio_set_dir( motor_ra_rev,GPIO_OUT);
	gpio_put(motor_ra_rev, 0);

	gpio_init(motor_rb_fwd);
	gpio_set_dir(motor_rb_fwd ,GPIO_OUT);
	gpio_put(motor_rb_fwd, 0);

	gpio_init(motor_rb_rev);
	gpio_set_dir(motor_rb_rev ,GPIO_OUT);
	gpio_put(motor_rb_rev, 0);
	
}

void setup_mode_btn(){
	gpio_init(btn_mode);
	gpio_set_dir(btn_mode, GPIO_IN);
	gpio_pull_up(btn_mode);
}

void test_all_h_bridge_pins(){
			///////LEFT
			///////////////////////////
			gpio_put(motor_la_fwd, 1);
			sleep_ms(on_delay);
			gpio_put(motor_la_fwd, 0);
			sleep_ms(off_delay);
			///////////////////////////
			gpio_put(motor_la_rev, 1);
			sleep_ms(on_delay);
			gpio_put(motor_la_rev, 0);
			sleep_ms(off_delay);
			///////////////////////////
			gpio_put(motor_lb_fwd, 1);
			sleep_ms(on_delay);
			gpio_put(motor_lb_fwd, 0);
			sleep_ms(off_delay);
			///////////////////////////
			gpio_put(motor_lb_rev, 1);
			sleep_ms(on_delay);
			gpio_put(motor_lb_rev, 0);
			sleep_ms(off_delay);
			
			//////RIGHT
			///////////////////////////
			gpio_put(motor_ra_fwd, 1);
			sleep_ms(on_delay);
			gpio_put(motor_ra_fwd, 0);
			sleep_ms(off_delay);
			///////////////////////////
			gpio_put(motor_ra_rev, 1);
			sleep_ms(on_delay);
			gpio_put(motor_ra_rev, 0);
			sleep_ms(off_delay);
			///////////////////////////
			gpio_put(motor_rb_fwd, 1);
			sleep_ms(on_delay);
			gpio_put(motor_rb_fwd, 0);
			sleep_ms(off_delay);
			///////////////////////////
			gpio_put(motor_rb_rev, 1);
			sleep_ms(on_delay);
			gpio_put(motor_rb_rev, 0);
			sleep_ms(off_delay);
}

void set_drive_mode(enum Mecanum_Drive_Mode mode){
	switch(mode){
		
		default:
		case STOP:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 0);
			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 0);
			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 0);
		break;

		case FORWARD:
			gpio_put(motor_la_fwd, 1);
			gpio_put(motor_la_rev, 0);

			gpio_put(motor_lb_fwd, 1);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 1);
			gpio_put(motor_ra_rev, 0);

			gpio_put(motor_rb_fwd, 1);
			gpio_put(motor_rb_rev, 0);
		break;

		case REVERSE:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 1);
			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 1);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 1);
			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 1);
		break;

		case ROTATE_CW:
			gpio_put(motor_la_fwd, 1);
			gpio_put(motor_la_rev, 0);

			gpio_put(motor_lb_fwd, 1);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 1);

			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 1);
		break;

		case ROTATE_CCW:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 1);

			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 1);

			gpio_put(motor_ra_fwd, 1);
			gpio_put(motor_ra_rev, 0);

			gpio_put(motor_rb_fwd, 1);
			gpio_put(motor_rb_rev, 0);
		break;

		case STRAFE_LEFT:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 1);

			gpio_put(motor_lb_fwd, 1);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 1);
			gpio_put(motor_ra_rev, 0);

			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 1);
		break;

		case STRAFE_RIGHT:
			gpio_put(motor_la_fwd, 1);
			gpio_put(motor_la_rev, 0);

			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 1);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 1);

			gpio_put(motor_rb_fwd, 1);
			gpio_put(motor_rb_rev, 0);
		break;		


		case DIAG_FL:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 0);

			gpio_put(motor_lb_fwd, 1);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 1);
			gpio_put(motor_ra_rev, 0);

			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 0);
		break;		

		case DIAG_FR:
			gpio_put(motor_la_fwd, 1);
			gpio_put(motor_la_rev, 0);

			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 0);

			gpio_put(motor_rb_fwd, 1);
			gpio_put(motor_rb_rev, 0);
		break;		

		case DIAG_BL:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 1);

			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 0);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 0);

			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 1);
		break;		

		case DIAG_BR:
			gpio_put(motor_la_fwd, 0);
			gpio_put(motor_la_rev, 0);

			gpio_put(motor_lb_fwd, 0);
			gpio_put(motor_lb_rev, 1);

			gpio_put(motor_ra_fwd, 0);
			gpio_put(motor_ra_rev, 1);

			gpio_put(motor_rb_fwd, 0);
			gpio_put(motor_rb_rev, 0);
		break;			


	}
}