Skip to content

GitLab

Commit b168fc46 authored by thcl's avatar thcl
Browse files

Implemented Direct Position Control

parent 939731a6
Hide whitespace changes
Inline Side-by-side
source_code/Teensy32/.vscode/extensions.json
View file @ b168fc46
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}
\ No newline at end of file
source_code/Teensy32/.vscode/settings.json
View file @ b168fc46
{
"terminal.integrated.env.windows": {
"PATH": "C:\\Users\\Matt\\.platformio\\penv\\Scripts;C:\\Users\\Matt\\.platformio\\penv;C:\\Program Files\\Dell\\DW WLAN Card;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;c:\\Program Files\\WIDCOMM\\Bluetooth Software\\;c:\\Program Files\\WIDCOMM\\Bluetooth Software\\syswow64;C:\\WINDOWS\\system32\\config\\systemprofile\\.dnx\\bin;C:\\Program Files\\Microsoft DNX\\Dnvm\\;C:\\Program Files (x86)\\Windows Kits\\8.1\\Windows Performance Toolkit\\;C:\\Program Files\\Microsoft SQL Server\\130\\Tools\\Binn\\;C:\\Program Files\\PuTTY\\;C:\\Program Files (x86)\\Xoreax\\IncrediBuild;C:\\Program Files\\MATLAB\\R2017b\\runtime\\win64;C:\\Program Files\\MATLAB\\R2017b\\bin;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Git\\mingw64\\bin;C:\\Program Files\\Git\\usr\\bin;C:\\Program Files (x86)\\Microsoft VS Code\\bin;C:\\Users\\Matt\\Anaconda3;C:\\Users\\Matt\\Anaconda3\\Library\\mingw-w64\\bin;C:\\Users\\Matt\\Anaconda3\\Library\\usr\\bin;C:\\Users\\Matt\\Anaconda3\\Library\\bin;C:\\Users\\Matt\\Anaconda3\\Scripts;C:\\Users\\Matt\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files (x86)\\Microsoft VS Code\\bin;C:\\Program Files\\Dell\\DW WLAN Card;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;c:\\Program Files\\WIDCOMM\\Bluetooth Software\\;c:\\Program Files\\WIDCOMM\\Bluetooth Software\\syswow64;C:\\WINDOWS\\system32\\config\\systemprofile\\.dnx\\bin;C:\\Program Files\\Microsoft DNX\\Dnvm\\;C:\\Program Files (x86)\\Windows Kits\\8.1\\Windows Performance Toolkit\\;C:\\Program Files\\Microsoft SQL Server\\130\\Tools\\Binn\\;C:\\Program Files\\PuTTY\\;C:\\Program Files (x86)\\Xoreax\\IncrediBuild;C:\\Program Files\\MATLAB\\R2017b\\runtime\\win64;C:\\Program Files\\MATLAB\\R2017b\\bin;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Program Files\\Git\\cmd;C:\\Program Files\\Git\\mingw64\\bin;C:\\Program Files\\Git\\usr\\bin;C:\\Program Files (x86)\\Microsoft VS Code\\bin;C:\\Users\\Matt\\Anaconda3;C:\\Users\\Matt\\Anaconda3\\Library\\mingw-w64\\bin;C:\\Users\\Matt\\Anaconda3\\Library\\usr\\bin;C:\\Users\\Matt\\Anaconda3\\Library\\bin;C:\\Users\\Matt\\Anaconda3\\Scripts;C:\\Users\\Matt\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Program Files (x86)\\Microsoft VS Code\\bin",
"PATH": "C:\\Users\\zatar\\.platformio\\penv\\Scripts;C:\\Users\\zatar\\.platformio\\penv;C:\\ProgramData\\Anaconda3;C:\\ProgramData\\Anaconda3\\Library\\mingw-w64\\bin;C:\\ProgramData\\Anaconda3\\Library\\usr\\bin;C:\\ProgramData\\Anaconda3\\Library\\bin;C:\\ProgramData\\Anaconda3\\Scripts;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files\\MiKTeX 2.9\\miktex\\bin\\x64\\;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Users\\zatar\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Users\\zatar\\AppData\\Local\\GitHubDesktop\\bin;C:\\Users\\zatar\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Users\\zatar\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;c:\\Cygwin64\\bin;;C:\\ProgramData\\Anaconda3;C:\\ProgramData\\Anaconda3\\Library\\mingw-w64\\bin;C:\\ProgramData\\Anaconda3\\Library\\usr\\bin;C:\\ProgramData\\Anaconda3\\Library\\bin;C:\\ProgramData\\Anaconda3\\Scripts;C:\\Program Files (x86)\\Common Files\\Oracle\\Java\\javapath;C:\\WINDOWS\\system32;C:\\WINDOWS;C:\\WINDOWS\\System32\\Wbem;C:\\WINDOWS\\System32\\WindowsPowerShell\\v1.0\\;C:\\Program Files\\MiKTeX 2.9\\miktex\\bin\\x64\\;C:\\WINDOWS\\System32\\OpenSSH\\;C:\\Users\\zatar\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Users\\zatar\\AppData\\Local\\GitHubDesktop\\bin;C:\\Users\\zatar\\AppData\\Local\\Microsoft\\WindowsApps;C:\\Users\\zatar\\AppData\\Local\\Programs\\Microsoft VS Code\\bin;c:\\Cygwin64\\bin;",
"PLATFORMIO_CALLER": "vscode"
}
}
\ No newline at end of file
source_code/Teensy32/src/encoder.cpp
View file @ b168fc46
......@@ -7,7 +7,7 @@
#define Z 7
int total_encoder_ticks = 8193;
int total_encoder_ticks = 8192;
int encoder_ticks_in_elec_rotation = 682; // floor(8193 / 12)=682
// has the motor been calibrated
......@@ -20,7 +20,7 @@ int Zoffset = 0;
// This is the difference in encoder tics from the Z index to
// the electrical encoder zero (which is found by powering the
// AB block and waiting for the motor to settle)
int hard_coded_offset = 860;
int hard_coded_offset = 910;
// how long in microsec (us) to delay between phases while calibrating
// the wheel
......@@ -145,6 +145,18 @@ void isr_index(void){
return;
}
// Holds last position to calculate difference between each state positions
int last_position_index_print = 0;
// Interupt Service routine
//interupts code when z-pin goes high and prints the difference in raw
//encoder position since the last z-pin high
void isr_index_print(void){
int myPosition = encoder.calcPosn();
Serial.println(myPosition - last_position_index_print);
last_position_index_print = myPosition;
return;
}
// This function will turn the motor blindly forward until it finds
// the zero index
// if this process fails will return -1 else will return 1
......
source_code/Teensy32/src/encoder.h
View file @ b168fc46
......@@ -3,6 +3,7 @@
void encoder_setup(void);
void isr_index(void);
void isr_index_print(void);
int wrap_wheel_tics(int in_position);
int get_wheel_position_ticks(void);
void debug_encoder(void);
......
source_code/Teensy32/src/main.cpp
View file @ b168fc46
......@@ -16,9 +16,11 @@ void setup()
// Hit space bar to start calibration
// Hit 'z' to run get hard coded offset
char prompt[] = ("\
SPACE TO START CALIBRATION, \n\
SPACE TO START CALIBRATION AND NORMAL OPERATIONS, \n\
Z TO GET HARDCODED OFFSET \n\
O TO START OPEN LOOP CONTROL\n");
O TO START OPEN LOOP CONTROL\n\
L TO LOCATE STATE POSITIONS\n\
D TO START DIRECT POSITION CONTROL\n");
int choice = get_input(prompt);
if(choice == 2){
do_hard_coded_offset();// run the set zero
......@@ -27,28 +29,14 @@ O TO START OPEN LOOP CONTROL\n");
{
open_loop_control();
}
// if the choice==1 just continue...
// while(1){
// Serial.println("AROUND THE WORLD");
// for (int i = 0; i < 6; i++){
// set_phase(1,0,0,0,1,0);
// delay(10);
// set_phase(1,0,0,0,0,1);
// delay(10);
// set_phase(0,1,0,0,0,1);
// delay(10);
// set_phase(0,1,0,1,0,0);
// delay(10);
// set_phase(0,0,1,1,0,0);
// delay(10);
// set_phase(0,0,1,0,1,0);
// delay(10);
// }
// turn_all_off();
// Serial.println("press space to try again");
// delayMicroseconds(100000); //0.1 sec delay
// block_until_spacebar();
// }
if(choice == 4)
{
locate_state_positions();
}
if(choice == 5)
{
direct_position_control();
}
int calibration_return = calibrate_encoder();
if(calibration_return == -1)
{
......
source_code/Teensy32/src/motor_ctrl.cpp
View file @ b168fc46
#include <Arduino.h>
#include "motor_ctrl.h"
#include "utils.h"
#include "encoder.h"
#define H1 6 //greenH
#define H2 21 //yellowH
......@@ -137,6 +138,96 @@ void open_loop_control(void)
}
}
//using hardcoded located state positions
void direct_position_control(void)
{
calibrate_encoder();
//attachInterrupt(digitalPinToInterrupt(7), isr_index_print, RISING);
block_until_spacebar();
while(1)
{
int e = get_wheel_position_ticks();
int s = 0;
if (e <= 174 ) {s = (1 % 6) + 1;}
else if(e <= 220 ) {s = (2 % 6) + 1;}
else if(e <= 397 ) {s = (3 % 6) + 1;}
else if(e <= 516 ) {s = (4 % 6) + 1;}
else if(e <= 633 ) {s = (5 % 6) + 1;}
else if(e <= 687 ) {s = (6 % 6) + 1;}
else if(e <= 858 ) {s = (1 % 6) + 1;}
else if(e <= 903 ) {s = (2 % 6) + 1;}
else if(e <= 1078) {s = (3 % 6) + 1;}
else if(e <= 1201) {s = (4 % 6) + 1;}
else if(e <= 1284) {s = (5 % 6) + 1;}
else if(e <= 1422) {s = (6 % 6) + 1;}
else if(e <= 1547) {s = (1 % 6) + 1;}
else if(e <= 1590) {s = (2 % 6) + 1;}
else if(e <= 1759) {s = (3 % 6) + 1;}
else if(e <= 1879) {s = (4 % 6) + 1;}
else if(e <= 1995) {s = (5 % 6) + 1;}
else if(e <= 2047) {s = (6 % 6) + 1;}
else if(e <= 2223) {s = (1 % 6) + 1;}
else if(e <= 2267) {s = (2 % 6) + 1;}
else if(e <= 2457) {s = (3 % 6) + 1;}
else if(e <= 2506) {s = (4 % 6) + 1;}
else if(e <= 2671) {s = (5 % 6) + 1;}
else if(e <= 2722) {s = (6 % 6) + 1;}
else if(e <= 2906) {s = (1 % 6) + 1;}
else if(e <= 2954) {s = (2 % 6) + 1;}
else if(e <= 3130) {s = (3 % 6) + 1;}
else if(e <= 3181) {s = (4 % 6) + 1;}
else if(e <= 3366) {s = (5 % 6) + 1;}
else if(e <= 3412) {s = (6 % 6) + 1;}
else if(e <= 3592) {s = (1 % 6) + 1;}
else if(e <= 3635) {s = (2 % 6) + 1;}
else if(e <= 3811) {s = (3 % 6) + 1;}
else if(e <= 3936) {s = (4 % 6) + 1;}
else if(e <= 3985) {s = (5 % 6) + 1;}
else if(e <= 4102) {s = (6 % 6) + 1;}
else if(e <= 4272) {s = (1 % 6) + 1;}
else if(e <= 4318) {s = (2 % 6) + 1;}
else if(e <= 4493) {s = (3 % 6) + 1;}
else if(e <= 4556) {s = (4 % 6) + 1;}
else if(e <= 4720) {s = (5 % 6) + 1;}
else if(e <= 4843) {s = (6 % 6) + 1;}
else if(e <= 4887) {s = (1 % 6) + 1;}
else if(e <= 5066) {s = (2 % 6) + 1;}
else if(e <= 5182) {s = (3 % 6) + 1;}
else if(e <= 5236) {s = (4 % 6) + 1;}
else if(e <= 5406) {s = (5 % 6) + 1;}
else if(e <= 5468) {s = (6 % 6) + 1;}
else if(e <= 5639) {s = (1 % 6) + 1;}
else if(e <= 5688) {s = (2 % 6) + 1;}
else if(e <= 5860) {s = (3 % 6) + 1;}
else if(e <= 5945) {s = (4 % 6) + 1;}
else if(e <= 6089) {s = (5 % 6) + 1;}
else if(e <= 6210) {s = (6 % 6) + 1;}
else if(e <= 6253) {s = (1 % 6) + 1;}
else if(e <= 6430) {s = (2 % 6) + 1;}
else if(e <= 6552) {s = (3 % 6) + 1;}
else if(e <= 6600) {s = (4 % 6) + 1;}
else if(e <= 6706) {s = (5 % 6) + 1;}
else if(e <= 6824) {s = (6 % 6) + 1;}
else if(e <= 6994) {s = (1 % 6) + 1;}
else if(e <= 7121) {s = (2 % 6) + 1;}
else if(e <= 7230) {s = (3 % 6) + 1;}
else if(e <= 7343) {s = (4 % 6) + 1;}
else if(e <= 7461) {s = (5 % 6) + 1;}
else if(e <= 7567) {s = (6 % 6) + 1;}
else if(e <= 7685) {s = (1 % 6) + 1;}
else if(e <= 7736) {s = (2 % 6) + 1;}
else if(e <= 7910) {s = (3 % 6) + 1;}
else if(e <= 8033) {s = (4 % 6) + 1;}
else if(e <= 8140) {s = (5 % 6) + 1;}
else if(e <= 8193) {s = (6 % 6) + 1;}
set_block_phase(s);
}
}
void debug_motor(void) {
while(1)
{
......
source_code/Teensy32/src/state_positions 12-4-18 modified.txt 0 → 100644
View file @ b168fc46
6:5
1:174
2:220
3:397
4:516
5:633
6:687
1:858
2:903
3:1078
4:1201
5:1284
6:1422
1:1547
2:1590
3:1759
4:1879
5:1995
6:2047
1:2223
2:2267
3:2457
4:2506
5:2671
6:2722
1:2906
2:2954
3:3130
4:3181
5:3366
6:3412
1:3592
2:3635
3:3811
4:3936
5:3985
6:4102
1:4272
2:4318
3:4493
4:4556
5:4720
6:4843
1:4887
2:5066
3:5182
4:5236
5:5406
6:5468
1:5639
2:5688
3:5860
4:5945
5:6089
6:6210
1:6253
2:6430
3:6552
4:6600
5:6706
6:6824
1:6994
2:7121
3:7230
4:7343
5:7461
6:7567
1:7685
2:7736
3:7910
4:8033
5:8140
\ No newline at end of file
source_code/Teensy32/src/state_positions 12-4-18.txt 0 → 100644
View file @ b168fc46
1:6994
2:7121
3:7230
4:7343
5:7461
6:7567
1:7685
2:7736
3:7910
4:8033
5:8140
6:5
1:174
2:220
3:397
4:516
5:633
6:687
1:858
2:903
3:1078
4:1201
5:1284
6:1422
1:1547
2:1590
3:1759
4:1879
5:1995
6:2047
1:2223
2:2267
3:2457
4:2506
5:2671
6:2722
1:2906
2:2954
3:3130
4:3181
5:3366
6:3412
1:3592
2:3635
3:3811
4:3936
5:3985
6:4102
1:4272
2:4318
3:4493
4:4556
5:4720
6:4843
1:4887
2:5066
3:5182
4:5236
5:5406
6:5468
1:5639
2:5688
3:5860
4:5945
5:6089
6:6210
1:6253
2:6430
3:6552
4:6600
5:6706
6:6824
\ No newline at end of file
source_code/Teensy32/src/utils.cpp
View file @ b168fc46
......@@ -52,10 +52,18 @@ int get_input(char * prompt)
{
return 2;
}
else if(incomingByte == 111)
else if(incomingByte == 111) //'o'
{
return 3;
}
else if(incomingByte == 108) //'l'
{
return 4;
}
else if (incomingByte == 100) //'d'
{
return 5;
}
}
}
......@@ -83,4 +91,27 @@ void do_hard_coded_offset(void)
block_until_spacebar();
}
}
//Locate a state, hold for half a second, print encoder value,
//repeat for each state
void locate_state_positions(void)
{
calibrate_encoder();
set_block_phase(1);
block_until_spacebar();
for (int i = 0; i < 12; i++)
{
for(int p = 1; p <= 6; p++)
{
set_block_phase(p);
delayMicroseconds(500000/5); //delay half-a-second
Serial.printf("%d:%d\n", p, get_wheel_position_ticks());
}
}
turn_all_off();
while(1)
{
}
}
\ No newline at end of file
source_code/Teensy32/src/utils.h
View file @ b168fc46
......@@ -5,9 +5,8 @@ void block_until_spacebar(void);
int get_input(char * prompt);
void do_hard_coded_offset(void);
int get_speed(void);
void locate_state_positions(void);
void direct_position_control(void);
......
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