Commit b1f48da8 authored by mattdr's avatar mattdr
Browse files

first try at butt controll not workinggit add *!

parent 34f3afac
......@@ -20,7 +20,8 @@ SPACE TO START CALIBRATION AND NORMAL OPERATIONS, \n\
Z TO GET HARDCODED OFFSET \n\
O TO START OPEN LOOP CONTROL\n\
L TO LOCATE STATE POSITIONS\n\
D TO START DIRECT POSITION CONTROL\n");
D TO START DIRECT POSITION CONTROL\n\
B TO START BUTT CONTROL (NON-OPTIMIZED)\n");
int choice = get_input(prompt);
if(choice == 2){
do_hard_coded_offset();// run the set zero
......@@ -37,6 +38,10 @@ D TO START DIRECT POSITION CONTROL\n");
{
direct_position_control();
}
if(choice == 6)
{
motor_butt_control_inefficient();
}
int calibration_return = calibrate_encoder();
if(calibration_return == -1)
{
......
......@@ -2,6 +2,9 @@
#include "motor_ctrl.h"
#include "utils.h"
#include "encoder.h"
#include "math.h"
#define pi 3.1415926535897932384626433832795
#define H1 6 //greenH
#define H2 21 //yellowH
......@@ -16,6 +19,10 @@ void turn_all_off();
int one_to_255(int x);
void set_phase(int high1, int high2, int high3, int low1, int low2, int low3);
void set_block_phase(int phase);
void motor_butt_control_inefficient(void);
void set_phase_float(float f1, float f2, float f3);
void motor_setup(void) {
// put your setup code here, to run once:
......@@ -113,6 +120,61 @@ void set_phase(int high1, int high2, int high3, int low1, int low2, int low3)
analogWrite(H3, high3);
}
void set_phase_float(float f1, float f2, float f3)
{
//deal with the potentiometer first
float potval = analogRead(A0);
potval = potval/1024;
potval = potval * 40;
int f1i = (int)(f1 * potval);
int f2i = (int)(f2 * potval);
int f3i = (int)(f3 * potval);
if(f1i == 0){
digitalWrite(L1, 1);
}
else{
digitalWrite(L1, 0);
}
analogWrite(H1, f1i);
if(f2i == 0){
digitalWrite(L2, 1);
}
else{
digitalWrite(L2, 0);
}
analogWrite(H2, f2i);
if(f3i == 0){
digitalWrite(L3, 1);
}
else{
digitalWrite(L3, 0);
}
analogWrite(H3, f3i);
}
void motor_butt_control_inefficient(void)
{
calibrate_encoder();
block_until_spacebar();
while(1)
{
int position = get_wheel_position_ticks();
float n = (position/8192) * 12 * 2 * pi;
n += 5*(pi/3);
// n is the mapped position in pi scale
float f1 = max(0, max(sinf(n), sinf(n - (pi/3))));
float f2 = max(0, max(sinf(n + ((2*pi)/3)), sinf(n + (pi/3))));
float f3 = max(0, max(sinf(n - ((2*pi)/3)), sinf(n - (pi))));
set_phase_float(f1,f2,f3);
}
}
//This just spins the motor using speed (potentiometer on pin A1)
//and torque (potentiometer on pin A0)
void open_loop_control(void)
......
......@@ -7,7 +7,8 @@ void set_block_phase(int phase);
void debug_motor(void);
void open_loop_control(void);
void set_phase(int high1, int high2, int high3, int low1, int low2, int low3);
void motor_butt_control_inefficient(void);
void set_phase_float(float f1, float f2, float f3);
......
......@@ -64,6 +64,10 @@ int get_input(char * prompt)
{
return 5;
}
else if (incomingByte == 'b') //'b'
{
return 6;
}
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment