From 2cee4f8803e7c7807e454fcfb0e2fd9b112eedf7 Mon Sep 17 00:00:00 2001 From: Andy Lindsay Date: Sun, 17 Sep 2017 20:12:50 -0700 Subject: [PATCH] Run up to 4 fb360 servos sans jitter (close issues 93 & 94) Issues closed: servo360 - increase number of servos supported #93 https://github.com/parallaxinc/Simple-Libraries/issues/93 servo360 - remove remove jitter from servo signal #94 https://github.com/parallaxinc/Simple-Libraries/issues/94 --- .../Motor/Servo360/Set Angles 1 servo.c | 35 +++++ ...et Angles.side => Set Angles 1 servo.side} | 12 +- .../{Set Angles.c => Set Angles 2 servos.c} | 0 .../Motor/Servo360/Set Angles 2 servos.side | 9 ++ .../Motor/Servo360/Set Angles 3 servos.c | 42 ++++++ .../Motor/Servo360/Set Angles 3 servos.side | 9 ++ .../Motor/Servo360/Set Angles 4 servos.c | 47 ++++++ .../Motor/Servo360/Set Angles 4 servos.side | 9 ++ .../Dev/IR Roaming at slower speeds.c | 40 +++++ .../Dev/IR Roaming at slower speeds.side | 9 ++ .../Dev/Test Encoder Connections (1).c | 28 ++++ .../Dev/Test Encoder Connections (1).side | 9 ++ .../Dev/drive_goto tests 3 servos.c | 34 +++++ .../Dev/drive_goto tests 3 servos.side | 9 ++ ...drive_goto tests with setMaxVelocity (1).c | 21 +++ ...ve_goto tests with setMaxVelocity (1).side | 9 ++ .../drive_goto tests with setMaxVelocity.c | 20 +++ .../drive_goto tests with setMaxVelocity.side | 9 ++ .../Motor/libservo360/API_PUBLIC.c | 30 ++-- .../Motor/libservo360/cmm/libservo360.a | Bin 14438 -> 15398 bytes .../Motor/libservo360/helpers.c | 3 +- .../Motor/libservo360/libservo360.c | 6 +- .../Motor/libservo360/servo360.c | 138 +++++++++++------- .../Motor/libservo360/servo360.h | 9 +- .../libabdrive360/cmm/libabdrive360.a | Bin 4780 -> 4780 bytes 25 files changed, 457 insertions(+), 80 deletions(-) create mode 100644 Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.c rename Learn/Examples/Devices/Motor/Servo360/{Set Angles.side => Set Angles 1 servo.side} (86%) rename Learn/Examples/Devices/Motor/Servo360/{Set Angles.c => Set Angles 2 servos.c} (100%) create mode 100644 Learn/Examples/Devices/Motor/Servo360/Set Angles 2 servos.side create mode 100644 Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.c create mode 100644 Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.side create mode 100644 Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.c create mode 100644 Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.side create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.c create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.side create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).c create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).side create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.c create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.side create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).c create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).side create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.c create mode 100644 Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.side diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.c b/Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.c new file mode 100644 index 00000000..883f4a24 --- /dev/null +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.c @@ -0,0 +1,35 @@ +/* + Set Angles.c + + Turns to and holds positions. + Behaves like a standard RC servo when it reaches a given angle. +*/ + +#include "simpletools.h" +#include "servo360.h" + +int main() +{ + // Connect is required before control. The first argument is the I/O pin + // connected to the white control line, and the second is the I/O pin connected + // to the yellow feedback line. + //servo360_connect(12, 14); + servo360_connect(13, 15); + pause(1000); + + //servo360_angle(12, 0); + servo360_angle(13, 0); + pause(1000); + + //servo360_angle(12, 45); + servo360_angle(13, -45); + pause(1500); + + //servo360_angle(12, 360 + 45); + servo360_angle(13, -360 -45); + pause(2500); + + //servo360_angle(12, 0); + servo360_angle(13, 0); + pause(1000); +} diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles.side b/Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.side similarity index 86% rename from Learn/Examples/Devices/Motor/Servo360/Set Angles.side rename to Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.side index b93b1d37..e94217a7 100644 --- a/Learn/Examples/Devices/Motor/Servo360/Set Angles.side +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 1 servo.side @@ -1,9 +1,9 @@ -Set Angles.c ->compiler=C ->memtype=cmm main ram compact ->optimize=-Os ->-m32bit-doubles +Set Angles 1 servo.c >-fno-exceptions ->defs::-std=c99 >-lm +>-m32bit-doubles >BOARD::ACTIVITYBOARD +>compiler=C +>defs::-std=c99 +>memtype=cmm main ram compact +>optimize=-Os diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles.c b/Learn/Examples/Devices/Motor/Servo360/Set Angles 2 servos.c similarity index 100% rename from Learn/Examples/Devices/Motor/Servo360/Set Angles.c rename to Learn/Examples/Devices/Motor/Servo360/Set Angles 2 servos.c diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles 2 servos.side b/Learn/Examples/Devices/Motor/Servo360/Set Angles 2 servos.side new file mode 100644 index 00000000..d8ad6a3c --- /dev/null +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 2 servos.side @@ -0,0 +1,9 @@ +Set Angles 2 servos.c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.c b/Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.c new file mode 100644 index 00000000..cc24442c --- /dev/null +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.c @@ -0,0 +1,42 @@ +/* + Set Angles.c + + Turns to and holds positions. + Behaves like a standard RC servo when it reaches a given angle. +*/ + +#include "simpletools.h" +#include "servo360.h" + +int main() +{ + // Connect is required before control. The first argument is the I/O pin + // connected to the white control line, and the second is the I/O pin connected + // to the yellow feedback line. + int s1 = servo360_connect(12, 14); + int s2 = servo360_connect(13, 15); + int s3 = servo360_connect(16, 6); + //int s3 = 0; + print("devcount = %d, s1 = %d, s2 = %d, s3 = %d\r", s1, s2, s3, devCount); + pause(1000); + + servo360_angle(12, 0); + servo360_angle(13, 0); + servo360_angle(16, 0); + pause(1000); + + servo360_angle(12, 45); + servo360_angle(13, -45); + servo360_angle(16, 45); + pause(1500); + + servo360_angle(12, 360 + 45); + servo360_angle(13, -360 -45); + servo360_angle(16, 360 + 45); + pause(2500); + + servo360_angle(12, 0); + servo360_angle(13, 0); + servo360_angle(16, 0); + pause(1000); +} diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.side b/Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.side new file mode 100644 index 00000000..5a2a4bb2 --- /dev/null +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 3 servos.side @@ -0,0 +1,9 @@ +Set Angles 3 servos.c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.c b/Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.c new file mode 100644 index 00000000..0ce143e2 --- /dev/null +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.c @@ -0,0 +1,47 @@ +/* + Set Angles.c + + Turns to and holds positions. + Behaves like a standard RC servo when it reaches a given angle. +*/ + +#include "simpletools.h" +#include "servo360.h" + +int main() +{ + // Connect is required before control. The first argument is the I/O pin + // connected to the white control line, and the second is the I/O pin connected + // to the yellow feedback line. + int s1 = servo360_connect(12, 14); + int s2 = servo360_connect(13, 15); + int s3 = servo360_connect(16, 6); + int s4 = servo360_connect(17, 7); + //int s3 = 0; + print("devcount = %d, s1 = %d, s2 = %d, s3 = %d, s4 = %d\r", s1, s2, s3, s4, devCount); + pause(1000); + + servo360_angle(12, 0); + servo360_angle(13, 0); + servo360_angle(16, 0); + servo360_angle(17, 0); + pause(1000); + + servo360_angle(12, 45); + servo360_angle(13, -45); + servo360_angle(16, 45); + servo360_angle(17, -45); + pause(1500); + + servo360_angle(12, 360 + 45); + servo360_angle(13, -360 -45); + servo360_angle(16, 360 + 45); + servo360_angle(17, -360 - 45); + pause(2500); + + servo360_angle(12, 0); + servo360_angle(13, 0); + servo360_angle(16, 0); + servo360_angle(17, 0); + pause(1000); +} diff --git a/Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.side b/Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.side new file mode 100644 index 00000000..e3669dde --- /dev/null +++ b/Learn/Examples/Devices/Motor/Servo360/Set Angles 4 servos.side @@ -0,0 +1,9 @@ +Set Angles 4 servos.c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.c b/Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.c new file mode 100644 index 00000000..dc80dc8c --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.c @@ -0,0 +1,40 @@ +/* + IR Roaming.c + + Use IR LED and IR receiver pairs as object detectors for robot roaming. + + http://learn.parallax.com/activitybot/roaming-infrared-flashlights +*/ + +#include "simpletools.h" // Library includes +#include "abdrive360.h" + +int irLeft, irRight; // IR variables + +int main() // Main function +{ + low(26); // D/A0 & D/A1 to 0 V + low(27); + + drive_setRampStep(12); // Max step 12 ticks/s every 20 ms + drive_setMaxVelocity(FOR_SPEED, 96); + + + while(1) + { + freqout(11, 1, 38000); // Check left & right objects + irLeft = input(10); + + freqout(1, 1, 38000); + irRight = input(2); + + if(irRight == 1 && irLeft == 1) // No obstacles? + drive_rampStep(128, 128); // ...full speed ahead + else if(irLeft == 0 && irRight == 0) // Left & right obstacles? + drive_rampStep(-128, -128); // ...full speed reverse + else if(irRight == 0) // Just right obstacle? + drive_rampStep(-128, 128); // ...rotate left + else if(irLeft == 0) // Just left obstacle? + drive_rampStep(128, -128); // ...rotate right + } +} diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.side b/Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.side new file mode 100644 index 00000000..53b7615f --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/IR Roaming at slower speeds.side @@ -0,0 +1,9 @@ +IR Roaming at slower speeds.c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).c b/Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).c new file mode 100644 index 00000000..363412da --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).c @@ -0,0 +1,28 @@ +/* + Test Encoder Connections.c + + Load to ActivityBot EEPROM. PWR to Position 2. + Gently turn each wheel by hand. Right wheel + blinks P27 LED, left wheel blinks P26 LED. + If not, encoders are not seeing wheel spokes. + Go to Extras/Troubleshooting section of tutorial. + + http://learn.parallax.com/activitybot/test-encoder-connections +*/ + +#include "simpletools.h" +#include "abdrive360.h" + +int main() +{ + low(26); + low(27); + while(1) + { + int ticksLeft, ticksRight; + drive_getTicks(&ticksLeft, &ticksRight); + set_output(26, ticksLeft % 2); + set_output(27, ticksRight % 2); + } +} + diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).side b/Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).side new file mode 100644 index 00000000..d4069cb5 --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/Test Encoder Connections (1).side @@ -0,0 +1,9 @@ +Test Encoder Connections (1).c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.c b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.c new file mode 100644 index 00000000..280707e9 --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.c @@ -0,0 +1,34 @@ +/* + Forward Stop Face Right.c + + http://learn.parallax.com/activitybot/go-certain-distances +*/ + + +#include "simpletools.h" +#include "abdrive360.h" + +int main() +{ + /* + #ifdef _console_ + console_start(); + suppressFbDisplay = 0; + #endif + */ + + drive_goto(192, 192); + servo360_connect(16, 6); + servo360_speed(16, 90); + drive_goto(-28, 28); + drive_goto(28 * 2, -28 * 2); + drive_goto(-28, 28); + drive_goto(-192, -192); + servo360_speed(16, -90); + + drive_goto(192, 128); + drive_goto(-192, -128); + drive_goto(128, 192); + drive_goto(-128, -192); + servo360_speed(16, 0); +} \ No newline at end of file diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.side b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.side new file mode 100644 index 00000000..7a656775 --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests 3 servos.side @@ -0,0 +1,9 @@ +drive_goto tests 3 servos.c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).c b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).c new file mode 100644 index 00000000..c23648bb --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).c @@ -0,0 +1,21 @@ + +#include "simpletools.h" +#include "abdrive360.h" + +int main() +{ + drive_setMaxVelocity(FOR_GOTO, 96); + drive_goto(192, 192); + drive_setMaxVelocity(FOR_GOTO, 64); + drive_goto(-28, 28); + drive_goto(28 * 2, -28 * 2); + drive_goto(-28, 28); + drive_goto(-192, -192); + + drive_setMaxVelocity(FOR_GOTO, 32); + + drive_goto(192, 128); + drive_goto(-192, -128); + drive_goto(128, 192); + drive_goto(-128, -192); +} \ No newline at end of file diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).side b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).side new file mode 100644 index 00000000..55b5ca5a --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity (1).side @@ -0,0 +1,9 @@ +drive_goto tests with setMaxVelocity (1).c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.c b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.c new file mode 100644 index 00000000..b8c3da5e --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.c @@ -0,0 +1,20 @@ +#include "simpletools.h" +#include "abdrive360.h" + +int main() +{ + drive_setMaxVelocity(FOR_GOTO, 96); + + drive_goto(192, 192); + drive_goto(-28, 28); + drive_goto(28 * 2, -28 * 2); + drive_goto(-28, 28); + drive_goto(-192, -192); + + drive_setMaxVelocity(FOR_GOTO, 32); + + drive_goto(192, 128); + drive_goto(-192, -128); + drive_goto(128, 192); + drive_goto(-128, -192); +} \ No newline at end of file diff --git a/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.side b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.side new file mode 100644 index 00000000..fe31f29e --- /dev/null +++ b/Learn/Examples/Robots/ActivityBot360/Dev/drive_goto tests with setMaxVelocity.side @@ -0,0 +1,9 @@ +drive_goto tests with setMaxVelocity.c +>compiler=C +>memtype=cmm main ram compact +>optimize=-Os +>-m32bit-doubles +>-fno-exceptions +>defs::-std=c99 +>-lm +>BOARD::ACTIVITYBOARD diff --git a/Learn/Simple Libraries/Motor/libservo360/API_PUBLIC.c b/Learn/Simple Libraries/Motor/libservo360/API_PUBLIC.c index 31a362a3..2d0a5962 100644 --- a/Learn/Simple Libraries/Motor/libservo360/API_PUBLIC.c +++ b/Learn/Simple Libraries/Motor/libservo360/API_PUBLIC.c @@ -123,6 +123,8 @@ int servo360_angle(int pin, int position) } lockclr(lock360); + + print("p = %d\r", p); } @@ -135,18 +137,6 @@ int servo360_goto(int pin, int position) while(lockset(lock360)); - /* - fb[p].speedTarget = 0; - fb[p].angleError = 0; - fb[p].erDist = 0; - fb[p].erDistP = 0; - fb[p].integralV = 0; - fb[p].derivativeV = 0; - fb[p].pV = 0; - fb[p].iV = 0; - fb[p].dV = 0; - fb[p].opPidV = 0; - */ offset = fb[p].angleTarget; target = position * UNITS_ENCODER / fb[0].unitsRev; @@ -189,7 +179,7 @@ int servo360_connect(int pinControl, int pinFeedback) if(fb360_findServoIndex(pinFeedback) != -1) return -5; - if(pinControl >= 28 && pinFeedback <= 28) + if(pinControl >= 28 && pinFeedback >= 28) return -6; if(devCount >= FB360_DEVS_MAX) @@ -198,14 +188,22 @@ int servo360_connect(int pinControl, int pinFeedback) while(lockset(lock360)); int result = -1; + /* + for(int p = 0; p < FB360_DEVS_MAX; p++) + { + print("fb[%d].pinCtrl = %d, fb[%d].pinFb = %d\r", p, fb[p].pinCtrl, p, fb[p].pinFb); + } + */ for(int p = 0; p < FB360_DEVS_MAX; p++) { - if(fb[p].pinCtrl == -1 && fb[p].pinFb == -1) + //print("fb[%d].pinCtrl = %d, fb[%d].pinFb = %d\r", p, fb[p].pinCtrl, p, fb[p].pinFb); + if( (fb[p].pinCtrl == -1) && (fb[p].pinFb == -1) ) { //fb[p].pinCtrl = pinControl; //fb[p].pinFb = pinFeedback; result = p; break; + //print("\r"); } } //lockclr(lock360); @@ -247,9 +245,13 @@ int servo360_connect(int pinControl, int pinFeedback) fb[p].angle = (fb[p].angleSign) * (fb[p].angleFixed - fb[p].pvOffset); fb[p].angleCalc = fb[p].angle; fb[p].angleP = fb[p].angle; + devCount++; lockclr(lock360); + print("devCount: %d, index: %d, p: %d\r", devCount, result, p); + print("fb[%d].pinCtrl = %d, fb[%d].pinFb = %d\r", p, fb[p].pinCtrl, p, fb[p].pinFb); + return result; } diff --git a/Learn/Simple Libraries/Motor/libservo360/cmm/libservo360.a b/Learn/Simple Libraries/Motor/libservo360/cmm/libservo360.a index 6b365f95962681659f9259f6d903ce3235a026d3..3138cfbf3eebfd6107a2a48f7e609ae68c50e716 100644 GIT binary patch literal 15398 zcmd5@3v^XinclfK$xTikoRE9Zp%UQIl7x034I#-z=;K0KpcN#Av{Y$GZjzf2n&gK2 zs1UhaDpResi>YI$gN_d}_O)Et*3nkS)dyBbi?-7`)3GySJEB&tGsRX!OI@ACeBU{H z-|QShV5)Vw*E;8X|G)qJ?|(nef6o5TKJzb$C40ABW?USeoR+jSwnQ42v^F+}!;Mas zeh!D1ENN+SOLQATh!!CVzx9V7=QRkSAK`u>^rQA(ARj2|8y|`%W9h-fh?WDJCsR%fRuArs_iacH4h-tD@sU{1q>57M z#Hd!f9}Vq^_1>tlQ1N8SQNM0*7`Cf!>fILaz43~{R5~`&8{Zfojt!0sjtr>W(ZRlz z8oo~9%xJ13(ViJf#aAa2!yU=kNUA@cT%8%|bz3NhfZf`b7)d7+L#kw7CcUd|m(yjg z5c;5FTRa_80=>yZDy5l#MzkkVgXzJW;#UkN(X&n$x@G$ZNBTB#0$erH7vHHgpv1=5 z@aU#=T<;&$Gc~s(G3uQqkZns%>RoW0&TESe^{RS$W25OzGQOd|-|6TKr-UQak+^cC zH_;bQiY|vKsg8t`tBNNx7+$v>?Dchte1&ZZ3_$Nlx;+`cNr}WqFpY;2y*FZRD;}!N zpo#rGqN^`{Q(Gc4g7NN4I}*;Nzh~3XU@vB;L#0L~gXh&{?7BoH-!EbCWv^SCct9cg90Sm%Bxdet)6} z&8V}?9%Gb-yvqoTpEQQ!sOjgUN9=Z^xGGy(W>QEMo~k~h8-=eH0%eQ)(6+6C7e=GC zM~ouJ=u#tEjm8zDamHvZ8fO&eo42{e6xE%C=&g;`U%JRT}CH zT?ZV>$pZHjxIMB!M6MU1#KWz3<6L(bM>LdZHAw(CM9?G$r%(=^Bzp|aFK53_*=MQ^ z$PdEW60fy)`&jwrmasTlc_h?X7TR3e@|=j=Ak40oLuRn+D=jyOU{~!oa75*C*uA#Y zFheqf6ZW8&UpI}C?-W`&^ly2|bUn<$xg4fVF-tgD&O|vu%OVRza5&Uygi6h*nYh<) zP1HA?{;{B%Cw4_-+x#6xm@YYA*g~0I<*%~O9{R?_q$!j0%s6`6;?-ATz{|lfe{$R6 z>p)ym6W(3Aq9%O%;{G-vP!#2w_AkC^r9(_^z+;##wU$vaev&iB%32#UCG}A|Yxj(m zEH)Rf%DlYT$;A!(aA2-5_Fy{v^0U$YZ3QS(5X#lx9Dl_>`?Z?SS-Hq=aqsWJiN;S9 zSh)0u{h?fBmbm9Bhq{B((fEPokQ_4ay$gA_ins_f6RhXWdv`!!r85);?<3UJER2!X zWx~vbvY~{Ptv_t#z-Y%Hgc3j7y4UQi%G$$KJ0Q7MwAP3~UJx45ZYx`7TDg9J;a>;J z%P~%N_Pd1~a{IyWmCSkK>GCS)H5nzjv~XX$UB;MT1UO(4No)XGzNU$bf)}2)U1Q< zvgfoZtz4&Z^p-kntksjsagX zo}9R=CYx~ibS9v%>y2iKNR61?HMK2W)7k4MSw{sX`e9oc=oy6zs0Yv~8x@<99!(<&T7vlONw;an;4lGfJgR~z9)6>w^ z?n39R^eqBaf_y-fBG^{@5hZvnki%O3>& zs78N;@|e#|EPnv>8jZdW^nQ*0I_O_&^i!Z`V6Ln3KL(xAXwk54Nd%|1Mx2^jiI(ux zgj4fUZ3U+mA(G`R6WmvKZ7tzl6KQJ(^;(a0!ymdd9!(B%v5{Z(4!oTevT}l&U1wbC zq3v>+n6DGfn7po5%a}7P^mKFOa_-MzsSTe1zrdRwsS<~u&Jw8_hrhys&OPmCCKB&w z{|pC;D~KikI%27(hgj+vAeMS|6QeBelaCTh`M)8S@}DA>@(&P0K8))V#4L#PZ5*oX zzYxo^FA&SJKP8s;(SH+5J#P|Ao2zgysJ5)q@CS*d%x%Qd&bx?ZU4KU`>*77gbsp0f zh-KL)h-F`u!G|b*kXYWM^N53P*@eWiu4ZCc*Sm>jT~`uI{nryq{aYjsL)8a0nOlja z&O3>v&QB6cop%$<{&`%J|E?zgtS0|MVm`2tj%xf@H2&{2{3h`%SKf~ar^*%+%eKrS zmTkFQ!(Sji&n^2PF&{Wczb2MCe@`s+aLoODxFXFWmVG;$SU$)aHU0`>**|NDW&f-r zmi@DpSoTkvxW?80L1J0ZvA{dgg0*0kO2LMZ;GUOM9**mU`Y#EcI;D@QuV$&u$HW zoLK7l46)Sn_ry}qKWO-?#8S`G#AqXj@82~3tHe_08^luQTf|Z)m#qHz?ikG^mhxd@ zDPK!0Rh6~HTBXYj!iFr)yYGT}mPV&mZIw#fR%vRaS!a8IgUUg28uX(t#_P#6a1y_)s%k>P!}{u<(;Nh3&g#G!8634&K8(z=|B+6Skt^#RIg`H zuB)X_ggS%kLo302N`y8b_Vqksbu-sjhpW4c$kL}paDDZq!6ktIE+R)o@Nxw5vem2h z+u8kg7c6iMcwR(ac5HBS_u1KfTJEC)+7AopcvYC4wTG)ZTf!nR-dAdNnd_T_qH2A! zX*%@fik{`>Zx~0&u=>(aBTS4!$%`U_2;c@YTWYVjeMb9!a|6SX0b{fe0hVa!I;S79 z!HxTzkf0kfdrCxJ6ro{;49%?1za1r>8xf*Mm~OPm6lIS;FkZM~tysQJK(%q$e5~q= zm`nC!<4wj^>x*`*EU+i?711)v@K2=KWDF(}G+{qe9YgRaE$oDi z`s@UvMz>D_+ ze5`2TxpyN#8Swen$^L!B?cJvkDQ;OP5Q0Nw%?ZPqB%+cCA`TWYc}nxN5WyC!GuU%A z0&1NNhe517UR->C@L2V#J+tb3drHvR$E@~TWu0%V+`-r6d{(EG<3M0g-8%PT7%_h6 z{p-bb+y3d)DHDTZcVlS2hM}n$&IOS5HBpl_DlWT$Uc)@>L`Kcmj0(54dqw0gF;-Tt zXv4G~_MM1!3@JAWcceV1H`>t%qn}1TE{+$Y93Mt?l2$4XDgZu{(nqhVd*n08c|9`a z&L!Fr#8dep)Dge|=+rxvNB$Nvhmfz5?wu9Sdp+GFPi~}&c0?6BIyu_G)$~kFDebaw zESHrt|9nqAS3fsP$ItxhL4Qi4v1rC>T7Fnl-!C;f{|(doq48~e(`0|DRmVp(e2<1_ zqAiqp5xfNw1B#Uz?j@FQ#y`;bCp5eXZKR$%PDgVNzR)dcT6WG~=!Vb^CjGf-T%$gLY(~yOtHK^Q9uNmfG`jFN z1>)d+1w7k>$3^+Ulf{+Yncw{e!ipN3L}h+Q{&7)q@MK|S_ZX-}GUxW`@Kij-F*tuY4rZhC%zt^;`$yEe9YpvkMjA0G8-fZF% z1-?ljCXGV_%CM5D;;`jA zSm1leXY6pl9+=(s42;!uZ;bDRKP!~ufSU9E$Lr6+m_IKg9~~qp&gnFjKU=2hf=FJU z*Poq9&g;)8cP{?y5c>_XoWit?$5j685axCoc`A9#RqRREbMtYg6VkhJ1mvpkQeeejLp~=d(tB_y{fHTjWMX&L*>*UXobyPN{Ac@D`{2XyIWq8r?r8OJ^~N!? zE{YeWjbkPCW<9>;)?0iDoOaj3v|l9W&d7hrpYAi@v^xHTUp4=PcDPS2XBR+JMB1I# zQ{MpY?IYN~K~R--wvIT%HceFEo$&GZYmSch8vsIwfDl{_nA|ne%`Hmsbw%_1 z^FZRMR#aII;DjcM=G9ri?iM0SPmdn4a&Hutm+zB>+$SVTF%X4IKNj<5ZoJ6MhA%__ zXxwnPrDl%CILa-GSvjA7i}59QF<>C%@nTsw!##7JnVYS*<|M~f7!{Xv&_8oMWOC)g z!+wEFB(8`yGnR7A-n$M6>lr(STFv1S>_ZA1hg7{aH#xS-+2b?0^e?d9DWe8hfVGQ9 zPsQnaU`WW>7Zdhn-cUyH>QoVOs_&q$&+baj=idc` z4$D~4e_e|GUk@x_CXB~dM6t!qe#w3iv7sx3_1I;Xpu-TS$~r--u;~JHj`<^m4$nJg zzT^Z6Ul_f^e$75s@3W8XF1B8(Ka#7i_ob@AF|QS`pE^aB)oyp&PZ;IO#oX+#uEh?kzh%^Dd_=P^~wbm79tDnVF!wg=h~pDcae~I)wciB38(0 zx)Ph8_KZYM2y8C>+1A5|P7XWK$v%X@plcx}VpaBdf$#ocX8?=R5u*@$%8C%I+a?g$ z3=S{wrCNf+492Av8~=PsH^v;BR?kNe%|vVxuYCRP4mQ?|dLKrF?X}ResGQNmd?>Du zw)pleWADL)y#;hUuF5h}CJq%5SA?I?o=7={|gE}Ej+RI`%Y zeNkRAGY?HzC>}N{LWz6bTNHUt8(3>@w%eg-ggc?IJNE_=>2O}R7-h_J`xt@^;~oqR zw_a}^FEZ}8hRyZp#Ntw{!hmf%&4QZlu{m+Pxm3=EnrH0ps!muzO-C!MjJcR#j_?Z@KqR=< zKY5(sdHEb*)e3swAs1p@Rf0^I>)1#+K11syjaN7)mB3Fc9b_oH2$+u^C08!}Ic1oX zocA4)a_906xfh1<%Xk2~SP%8_naanbsuNIjgYg}wlS#?`cHtS%=lTx$KdPdr8~5Cz z8U-5NCptBnwr$twkXAe2@xq|HWIpLeJs+5BV`lTH630)q;i?jKufd_`x2sHCna@YX ze95DJzLVe`F9l-bvXIOM&`8tR=RqJZtPFS zcl@#bU>Qn$z>Qux0WBxKge_qZO5A*&L z@8S!A zX-@~pi!~YNJCSTV*Jw(QvqrnXExVO`Xo%X+iKVOP)1#PePJ zQDD_Sp8zhdb@`vu@I4xSM3es&alI@57V$zCFUA-vof|aVL(GLcQk?iNT>K$oz9=Al zoS2gx=>V~OUwBKCsq}--35gUUmg9RBvDC9glj+f9ZrAW(P5xgs{GXc4>l$`;v&p)8 zP`{dQabh`Fw-L{DZR2iZW!s&^buRyt#232w^Tcv)A0n1>``g5P8AEb*rv+U6f0EXK Ae*gdg delta 4991 zcmbuDdvH|M9mmhzcS1HSkL(5rWP!K|)`Vn%1Vuv|ihZDzo{C2+ zrKghHlycI17ykSHwOva6|Eaggxa_g+@a4r+D!p<_b!Doxv8}yDrB*j?n3ZW?+h(oz zrJdnh3u2=@l~b*=SCx5w73iJdf84MR1`7k7@#r^{vbqw{xzBonF_pWfI<8LTzY%R7 z5?zy1eN5FXRgp~f^O1tgPpg-zf=uEUNOh84bMDne%n)aR^+GUVwFd7tmRJ{p73SD@ zYtD?K_`~JtSxT+JTy>NV-(J3<-WCNNe-Gs5^$pK!NqE}(kei6cI$|r^^U_L1o1^PH zUr?LI47j(IN87^#IR<2IxVwL?Qu~ABRPH@BWeVDenl&Yn^zFu&b46xN(L4pcS*1Vr zM$OW?o-cS!|BZzTRXXlCkJ`>1bT&oOd#;I^J-cciLdDWLxQ<8CDh{siS!cZ)y0)Zt zo(e|Hf{ua=x`N)Gi#3-bb!JUTVVEJc& zLuCNez0es=AA^2c(;qlh?JEcODcT<}IzkehPU=GFFJ(s9Kr$HDF>{=xE(+yKX7&G8KMi<;gC{d-Nn2|Xh)7(i9btE#~zS#5nj^hxWxMs4rO zNYKEr*lRDIJ}n=~n`5IILFTs#R*dB3Q->vyyjW4R*+{PcX0pUtNtQTRn*(w1$+PQn z%DzW7rNUzrQsD`*H2f1X8jIt8m>k9h_BxW=*xO{O`vt2e~?hmE0-Kyk7J zs?fNKEP)y{zJ)9uT1A#Px05B#MvXhj66aBq0;A+(ds55TN0y+kk|pRb$rAK;*+FjuT9)*Scgk9#`D(XvWbD&81*5;-uzD)1Dn5%2@h`A z{Efxd#j;ZWYOIb^*0916){gLOYr~|tQDN<#G;>9(-4$owS_ytx(;LB;G`$Tx6Q4X9 z_rY%5AJFuE=slW#6S~`~x$=xjr~3ir+>+5DXBz_Zi%=wfei5q3L0n+hA-NB#fy|09 zo-5pkwt|c}e9oA)0sjVaP(H->QOFbDLY6>}Y89R(OQ7d8|0`ta(67l7=Qm`Db3!Zs z2^n#?7oKDLZco2th6KvNw77xt$r5NZS(alt8G%YrZ;Ix(w=;?JP5Pxn3&|2^nOmOg zjfW5hY69QoW`MVdAE@j-4`t%uA6qY8Rje)IDamA|eaOGnXBC#u9qPT#J|N?KJodBP zaIMu}URw59Y(*MlZVnBJ1cQA_b>)WfHpM;d|GWs{CjU%pdHE>o2j#`XLJ_l1<#pk# zyBrl`8Bg!tia&a$E@`&=iMp}(qQ8wTkNx?g%J2XAo%p0ja`WM~FK6>3Eb;2N^_x$d zC#-*0wR=AG+GpaOoIJ1nTEu|4;nm zH(XZ*>N(Ina z8uR_aw#KC(OJ4!~Bdz?5to-Yt=dv=5BW%tpaG$R0@Gw)W+wcQfrR*!t;L~^zpTZ;g z)z(|yvDVh=dHf|^r&3LsyVU%-i|WW z9~BdwGKW}(&kA-DY= FB360_DEVS_MAX) + //print("p = %d, ", p); + if(p >= (FB360_DEVS_MAX)) { return -1; } diff --git a/Learn/Simple Libraries/Motor/libservo360/libservo360.c b/Learn/Simple Libraries/Motor/libservo360/libservo360.c index a29137fb..b7737ccb 100644 --- a/Learn/Simple Libraries/Motor/libservo360/libservo360.c +++ b/Learn/Simple Libraries/Motor/libservo360/libservo360.c @@ -16,11 +16,11 @@ int main() { -//int state = servo360_setMaxSpeed(12, 180); -// print("speedLimit = %d\r", speedLimit); + //int state = servo360_setMaxSpeed(12, 180); + // print("speedLimit = %d\r", speedLimit); //servo360_setramp(12, 2); -// print("rampStep = %d\r", rampStep); + // print("rampStep = %d\r", rampStep); servo360_connect(12, 14); servo360_connect(13, 15); diff --git a/Learn/Simple Libraries/Motor/libservo360/servo360.c b/Learn/Simple Libraries/Motor/libservo360/servo360.c index 08c9084f..68f588b3 100644 --- a/Learn/Simple Libraries/Motor/libservo360/servo360.c +++ b/Learn/Simple Libraries/Motor/libservo360/servo360.c @@ -21,12 +21,15 @@ int *servoCog; volatile int lock360; volatile int devCount; -static volatile int t360; -static volatile int dt360; +volatile int t360; +volatile int t360slice; +volatile int dt360; volatile int angleSign = CCW_POS; volatile int cntPrev; +volatile int dt360fbSlice; +volatile int dt360spSlice; -servo360 fb[2]; +servo360 fb[FB360_DEVS_MAX]; void fb360_run(void) @@ -35,6 +38,7 @@ void fb360_run(void) cntPrev = CNT; pause(500); } + void fb360_end(void) { @@ -43,75 +47,101 @@ void fb360_end(void) } -void fb360_servoPulse(int p) +void fb360_setup(void) { - //high(26); - int pin = fb[p].pinCtrl; - low(pin); - //low(12); - PHSA = 0; - FRQA = 0; - CTRA = (4 << 26) | pin; - FRQA = 1; - PHSA = -(15000 + fb[p].speedOut) * (CLKFREQ/10000000); - //low(26); - /* - low(27); - PHSB = 0; - FRQB = 0; - CTRB = (4 << 26) | 27; - FRQB = 1; - PHSB = -(15000 + 500) * (CLKFREQ/10000000); - */ - while(get_state(pin)); - CTRA = 0; - PHSA = 0; - FRQA = 0; -} + for(int p = 0; p < FB360_DEVS_MAX; p++) + { + fb[p].pinCtrl = -1; + fb[p].pinFb = -1; + } + + devCount = 0; + + lock360 = locknew(); + lockclr(lock360); + + dt360 = CLKFREQ / FB360_FREQ_CTRL_SIG; // 20 ms + dt360fbSlice = 4 * dt360 / 5; // 16 ms + dt360spSlice = dt360 / 10; // 2 ms + t360 = CNT; + t360slice = t360; +} void fb360_mainLoop() { fb360_setup(); - //low(27); - - //int p = 0; - while(1) { waitcnt(t360 += dt360); - for(int p = 0; p < devCount; p+= 1) - //if(devCount > 0) + for(int p = 0; p < FB360_DEVS_MAX; p++) { - //toggle(27); - fb360_checkAngle(p); - fb360_outputSelector(p); - //fb[p].speedOut = 0; - fb360_servoPulse(p); - //toggle(27); + if(fb[p].pinCtrl != -1 && fb[p].pinFb != -1) + { + fb360_checkAngle(p); + fb360_outputSelector(p); + } + } + for(int p = 0; p < FB360_DEVS_MAX; p++) + { + if(p % 2 == 1) + { + waitcnt(t360 + dt360fbSlice + ((p/2) * (dt360spSlice))); + fb360_servoPulse(p - 1, p); + } } } } +// - -void fb360_setup(void) +// +void fb360_servoPulse(int p, int q) { - - for(int p = 0; p < FB360_DEVS_MAX; p++) + int pinA = fb[p].pinCtrl; + int pinB = fb[q].pinCtrl; + + if(pinA != -1) { - fb[p].pinCtrl = -1; - fb[p].pinFb = -1; - } - - devCount = 0; - - lock360 = locknew(); - lockclr(lock360); + low(pinA); + PHSA = 0; + FRQA = 0; + CTRA = (4 << 26) | pinA; + FRQA = 1; + PHSA = -(15000 + fb[p].speedOut) * (CLKFREQ/10000000); + } + + //if(pinB != -1 && q < FB360_DEVS_MAX) + if(pinB != -1) + { + low(pinB); + PHSB = 0; + FRQB = 0; + CTRB = (4 << 26) | pinB; + FRQB = 1; + PHSB = -(15000 + fb[q].speedOut) * (CLKFREQ/10000000); + } + + if(pinA != -1) + { + while(get_state(pinA)); + CTRA = 0; + PHSA = 0; + FRQA = 0; + } - dt360 = CLKFREQ/50; - t360 = CNT + dt360; -} + //if(pinB != -1 && q < FB360_DEVS_MAX) + if(pinB != -1) + { + while(get_state(pinB)); + CTRB = 0; + PHSB = 0; + FRQB = 0; + } +} +// + + void fb360_waitServoCtrllEdgeNeg(int p) diff --git a/Learn/Simple Libraries/Motor/libservo360/servo360.h b/Learn/Simple Libraries/Motor/libservo360/servo360.h index fc6b4584..e475339e 100644 --- a/Learn/Simple Libraries/Motor/libservo360/servo360.h +++ b/Learn/Simple Libraries/Motor/libservo360/servo360.h @@ -34,7 +34,7 @@ #define GOTO 3 #define MONITOR 4 #define FB360_FREQ_CTRL_SIG 50 -#define FB360_DEVS_MAX 2 +#define FB360_DEVS_MAX 4 #define FB360_RAMP_STEP 10 * 4096 / 360; @@ -84,6 +84,7 @@ void fb360_setup(void); void fb360_mainLoop(); void fb360_outputSelector(int p); +void fb360_servoPulse(int p, int q); void fb360_waitServoCtrllEdgeNeg(int p); int fb360_dutyCycle(int p, int scale); int fb360_crossing(int current, int previous, int units); @@ -125,6 +126,10 @@ int terminal_checkForValue(fdserial *connection, int *value); //extern fdserial *term; extern int *servoCog; extern volatile int lock360; +extern volatile int t360; +extern volatile int t360slice; +extern volatile int dt360fbSlice; +extern volatile int dt360spSlice; extern volatile int devCount; //extern volatile int SCALE_DEN_V; // Velocity control system denomenator @@ -194,7 +199,7 @@ typedef volatile struct servo360_s { volatile int angleTarget; } servo360; -extern volatile servo360 fb[2]; +extern volatile servo360 fb[FB360_DEVS_MAX]; // console #ifdef _console_ diff --git a/Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/cmm/libabdrive360.a b/Learn/Simple Libraries/Robotics/ActivityBot360/libabdrive360/cmm/libabdrive360.a index 69e4faf61d312a62ace87d0884624e2ea40df2c1..a20b6b9175e9a1cc52e8ee2abf85ff9e8f13cba7 100644 GIT binary patch delta 29 fcmZ3Zx<++^44b)uv7x!)Mx{lJQ0C@SjE@8We<28` delta 29 gcmZ3Zx<++^44bKisiCRGMx{lJ5T^O&Q;d%U0DyW3zyJUM