RC Truck and Construction - 90 ton 1/14 metal excavator scratch build w/embedded PC

RC Truck and Construction (https://www.rctruckandconstruction.com/index.php)

- Construction Equipment (https://www.rctruckandconstruction.com/forumdisplay.php?f=6)

- - 90 ton 1/14 metal excavator scratch build w/embedded PC (https://www.rctruckandconstruction.com/showthread.php?t=10796)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Mounting the motor controllers (ESCs)

I wanted to mount the motor controllers in a compartment that had plenty of air flow, as they are the components that need the most cooling. I put them where the radiators are on the real version and I was able to add four controllers that can then control eight motors of up to 30 Amps each. Only six is required for the main motors. The two extra channels will be used for attachments and a surprise! :-)

I also wanted to be able to get easy access to the controllers and easily replace one if I need to. So I made a seat and four slot-in module mounts:

http://d1t2tbzmskak12.cloudfront.net...IMAG1015_a.jpg

The motor controller modules each slide down into this seat:

http://d1t2tbzmskak12.cloudfront.net...IMAG1015_b.jpg

And the air flow will be ok I think with the vents as they are placed on the original:

http://d1t2tbzmskak12.cloudfront.net...IMAG1015_c.jpg

http://d1t2tbzmskak12.cloudfront.net...IMAG1015_d.jpg

Each module consists of the motor controller as described previously and a 3D-printed (15% infill) module mount that attaches to the controller with two M3 nylon bolts. I use nylon here to avoid any kind of electrical connection in the screw.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1029.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1030.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1032.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1034.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1035.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1036.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1037.jpg

Each module is secured with one self-tapping screw. Only this screw needs to be removed to slide the module up and out again:

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1040.jpg

The seat attaches to the base plate also with just one M4 50mm bolt:

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1041.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1044.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1046.jpg

I'm pretty happy that I got all the motor controllers into this one spot, because it leaves more room for the other stuff. Even as large as this model is, it's going to get tight when everything is in place.

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Those mounts turned out fantastic!

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Very interesting design, really like all the printed parts, well done!!!

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Thanks, DADSGARAGE! I'm following your build with excitement! :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I really wanted this excavator to have proper sound. Deep and powerful bass. Not toy-like. I looked around a bit to try and find a large speaker that would fit, but came up empty. So I printed one instead. My biggest print to date. 21 hours and 290 grams of ABS. :-) (25% infill)

The speaker driver is this one:
- Pioneer TS-G1045R 4-Inch 210 W 2-Way

On the back there is a speaker terminal that I bought on Ebay:
- 44.8x21mm 2pin Red and Black Push Type Speaker Terminal

And then there are four self-tapping screws in the front, two on the terminal, and three that fasten the speaker box to the base plate.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1054.jpg

I cut off the mounting tabs on the speaker driver, because I needed the screw holes to be closer to the cone. The flange on the self-tapping screws is now what holds the speaker driver in place:

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1055.jpg

Soldered the wires that came with the speaker to the terminal:

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1061.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1062.jpg

Attaching the speaker box to the base plate with three self-tapping screws. I added a soda can too, as I've seen a few others do that lately. My build is dwarfed by DADSGARAGE's and Cooper's though, but still it's not that small. :-)

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1064.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1065.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1068.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1069.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1071.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1073.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Great job with all of the 3D printing, it's very fun to see what people can come up with and easily create. If the sound doesn't seem quite correct with your setup, put some insulation inside of the box like bed top foam or something like that.

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Hi Trucker_Jo!

Yes, it's pretty cool, but this print was a pain. I started it and it ran for 5-8 hours four times before I got one successful print. The problems were lifting edges, support coming loose, support lifting and SD card error. :-)

I'm definitely going to add some dampening material inside. Initially, I tried to print something like that, but it didn't work that well, and I decided to do it the traditional way, and then completely forgot about it. Thanks for reminding me! :-)

Here are some images of my attempts at printing the inner dampening:

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1077.jpg

Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Interesting, thanks for showing us the details!

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Overall looks to be pretty smooth... some springiness when stick is pushing hard, but still very impressive for electric rotation. Well done Stein! :cool:

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Thanks, Joe! That means a lot, coming from you!

I've not seen the springiness in the stick before since I haven't been pushing anything hard, but it's caused by the combination of the 6kg gas spring and the backlash in the gears. When there is no load, the gas spring pushes the stick to hang on the gears. when the stick is pushing hard, it takes over and the stick is pushed by the gears. When lifting the arm up, the gas spring is free to push the stick back over the backlash and to have it hang on the gears again.

I think I'll try to just lower the force on the gas spring to get rid of this problem or make it less of a problem. I'll try with a 3kg gas spring first. I though bigger would be better, because the gas spring offsets the force to give the stick more pull force. But it doesn't need that, and it's causing this problem, so it should be smaller.

Best regards,
Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

If someone is wondering why the excavator is standing on planks, then the reason is that I didn't want it to get dirt into every corner of the track when it's going right back onto my work bench. :o

Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I don't recall reading where you added a gas spring, but great idea to take up the slack in the gear back lash to keep the movement tight. ;) :cool:

Whether 3kg or 6kg gas spring, the difference in length of backlash travel will always be the same... might as well leave the 6kg in place.

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Yes, true, I just thought the 3kg would be less violent when it engages. It should probably be a pulling spring instead of a pushing spring so that the backlash is only seen when pushing hard with the stick and not when pulling hard. I'll see what I can do about it.

I also found that the set screw was loose, so the backlash is only about half of what you see in the video when the set screw is tight.

I have done a few things that I haven't documented here yet. The gas spring on the stick, a gas spring on the bucket, a mount for the CPU (the Raspberry Pi) and angle sensors for the bucket and stick. I'll get those updates up here shortly. :-)

Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Quote:

Originally Posted by SteinHDan (Post 143004)

I also found that the set screw was loose, so the backlash is only about half of what you see in the video when the set screw is tight.

Do you have flat spots on the shaft for the set screws to anchor to? Blue locktite is adequate to keep set screws tight! But the locktite will also seep around shaft & mounted part too making it a real bytch! to get apart again. :eek: Maybe you don't want to add that just yet during testing phases, eh? :D

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Yep, that's good advise.There is a flat spot, but I just hadn't tightened it and I only added the left set screw, not the right. Lot's of screws loose at this point, like you say, it gets taken apart all the time when when building and testing like this. I'll get it all tight at the final assembly and add some blue locktite for good measure! :-)

Stein

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Added a simple mount for the brain (CPU) of the excavator. At the same time I replaced the Raspberry Pi 2 with the version 3. It's mostly the same, but a little bit quicker and has WiFi and BlueTooth built in.

- Raspberry Pi 3

The mount is very simple. Four self-tapping screws to hold the PCB and then two nylon M4 bolts to hold the mount on the base plate.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1084.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1089.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1092.jpg

The audio (engine sound) has its output through that 90-degree 3.5mm jack on the top.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1094.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Bolting together the arm

The arm sandwich of 3mm aluminum sheets with 3D-printed ABS in the middle is held together by 34 M4 countersunk stainless steel bolts and sleeve nuts.

There are different length bolts used throughout the arm:
- 23 x 50mm (for the main part)
- 5 x 75mm (80mm cut to length) for the stick hub
- 4 x 80mm for the stick mount
- 2 x 55mm for the stick angle sensor mount

Ebay links:
- Bolts: http://www.ebay.com/itm/231787670429
- Sleeve nuts: http://www.ebay.com/itm/191771467782

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1100.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1101.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I added a gas spring from the boom to the stick. I only found the correct length, stroke and force on Aliexpress:

- 6kg (60N) gas spring: http://www.aliexpress.com/snapshot/7601366858.html

The sleeves and spacers are cut from 8mm and 12mm stainless steel tubes. The pin is also gotten from Ebay:

- 8mm (ID 4mm) tube cut to 13mm length: http://www.ebay.com/itm/121600172085
- 12mm (ID 8mm) tube cut to 4mm legnth: http://www.ebay.com/itm/121762512929
- 8x30mm pin: http://www.ebay.co.uk/itm/121851516316

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1102.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1103.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1105.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1104.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I also added a gas spring to the bucket, mostly for the visual appearance as the bucket is already way more powerful than it needs to be.

The spacers are made the same way as previously, cut tube to length.

Ebay links:
- 8mm (ID 6mm) tube cut to 4mm length: http://www.ebay.com/itm/121762445745
- 12mm (ID 8mm) tube cut to 4mm legnth: http://www.ebay.com/itm/121762512929
- 6x30mm pin: http://www.ebay.co.uk/itm/121868709865
- 8x80mm pin: http://www.ebay.com/itm/291511678072
- Eyelets: http://www.ebay.com/itm/151838223022

The lower eyelet is drilled out to 8mm to fit the 8mm pin in the bucket linkage.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1128.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1130.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1135.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1137.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1147.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I've had one issue with the operation, and that is that I need some way to stop the motors at the end of travel. I could have done this with limit switches like I did for my MAN truck build:
- http://www.scale4x4rc.org/forums/showthread.php?t=80393

An alternative is to use angle sensors in each joint, and I've selected this method for this build because it also allows for a host of other cool applications and features:

- End stops
- Stall detection
- Recording of movement
- Replay of movement
- Full servo capability at the joint
- Alternative control modes (e.g. for beginners), like absolute bucket movement forward/backward and up/down, not angle control
- Automatic bucket leveling
- Automatic grading
- GPS assisted digging
- GPS controlled digging
- .. and probably more.

So I've started to add some angle sensors to the arm. First the stick and the bucket. (This approach could also very well be used on hydraulic arms, btw.)

I'm reading the sensor on the corresponding motor controller. That way, the control of the motor based on the angle sensor can be completely controlled on the controller without having to bother the main CPU.

As I was already modifying the motor controller, I added some heat sinks for good measure, but those are most likely not required at all.

Ebay link:
- 18x18x13.5mm heat sink: http://www.ebay.com/itm/361352931009

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1152.jpg

To read the sensor, I'm using the unused analog inputs 4 and 5 on the Arduino. I've added longer pins to these positions so that I can attach wires on the front side. I did the same for GND and 5V.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1154.jpg

The angle sensor itself is just a 10k Ohm linear potetiometer. I'm using a thumb wheel version to get it as flat as possible. I bought these ones at SparkFun:

- Thumbwheel potentiometer, 10k linear: https://www.sparkfun.com/products/retired/11173

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1159.jpg

The potentiometer attaches to the hub adapter using one very small self-tapping screw.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1161.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1168.jpg

The hub adapter has a D-shape that fits exactly onto the D-shape of the gear motor axle:

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1169.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1170.jpg

I've printed a part that mocks up a stick mount that has two purposes:
- Cover and hold the angle sensor
- Be a base when running the control an power lines to the stick.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1172.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1174.jpg

The sensor connector has three pins, that connect to the Arduino cable from the first step:
- Pin1: GND
- Pin2: Sensor output (0-5V)
- Pin3: 5V

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1177.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1179.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1188.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Very cleaver.... you and this build amaze me ! Keep it up...

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Thanks, Trucker Al!

I really like this forum. The positive atmosphere and the combined competence of everyone here fuel many of the builds, I think.

Best regards,
Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Added angle sensor to the bucket.

The bucket angle sensor attaches in the same way as the stick angle sensor, but the cover is different. Its only purpose is to hold the sensor in place and protect it. Not my prettiest piece of work, but it'll get the job done.

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1195.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1204.jpg

http://d1t2tbzmskak12.cloudfront.net...r/IMAG1206.jpg

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

that's cool dan!! you lost me with all the computer stuff but still a real cool build

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

As I have written before your build is great , ton of info for us beginners, can I ask a couple of questions, what DP or MOD did you base your swing gears on, I am trying to cut a pair but cannot seem to come up with a workable combination,( do not have cnc) I am also very interested in 3D printer work, about how long does it take to produce your track pads and if I may ask what make of printer do you use? Thanks for any info you may have.
Terry

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Excellent work, you are on a whole other level.

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Hi Crusher!

I used the Spur gear generator add-in in Fusion 360 to generate the gear, but there are also many online generators:
- http://geargenerator.com/
- https://woodgears.ca/gear_cutting/template.html
- http://hessmer.org/gears/InvoluteSpurGearBuilder.html

For my main gear, these are the parameters:
- Diametral pitch: 3.175 cm (1.25 inches)
- Number of teeth: 44
- Pressure angle: 20

For the pinion gear it's the same parameters but just 7 teeth.

Then I used the 50 rpm (no load) gear motor to drive these gears to get the wanted swing speed:
- 50 RPM * ( 7 / 44) = 7.95 RPM (no load)

The swing speed from the CAT 390F official specifications is 6.2, so with load I believe my speed matches that pretty well.

I did it like this mostly because I couldn't find a gear ring that would fit and also in a way that I could cut it on my CNC. I wanted an internal gear, but my cutting bits for my CNC aren't small enough to cut inner gears, so it had to be an external gear. But I think external gears look cool, though, so all was not lost. :-)

Later I've been thinking that I should rather not cut the gears myself, but use some more time to find gear rings on the net that I could use.

I'm really happy about the track pads. They really hold up great! Not one have broken so far, even as the machine now weighs 30kg (60lbs). I was initially planning to mill them all from aluminum, but now I'm thinking that if some break, I just print some new ones.

On my 3D printer, it takes about two days of continuous printing to print 80 track pads. I have a Makerbot Replicator 2X.
- http://store.makerbot.com/replicator2x

But that's quite an old 3D printer. I think these days, you can get better offers. The only important thing is that you can print ABS, because it's so much tougher than PLA (less brittle). And you need a heated build plate for ABS printing. Maybe one of these could do the job:
- Ultimaker 2
- Zortrax M200 Pro
- FlashForge Creator Pro

I've not tried any of them, but I've heard some good things about them and the specs look ok. There's probably lots of reviews on youtube.

Best regards,
Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Thanks, BigFord and MarkTurbo! :-)

Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

That thing is a beast!

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Thank you very much for your reply re swing gears, much appreciated.
Terry

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I love that you made a system that you can use a controller with, I was wondering for awhile if it was possible within reason and you have proved (and gone far beyond) that it is.
Ill be copying this system as well as some more ideas of yours when I start my build a ways from now (which i will put in the build page)
Its really helpful with all the links to everything you have done and detailed comments on it, thanks for that, it will help me tons, I cant wait for the final version =)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Thank you for those kind and encouraging words, Tk421reporting!

I'm looking forward to following your build!

Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

I'm working on the house body now, and even though I wrote this in another thread already, it really belongs in this thread.

To try to get the right dimensions on all the small scale features, I typically try to find a couple of measurements from a spec sheet, and then scale a photo or diagram as a canvas in Fusion 360 to match the known sizes.

E.g. from the spec sheet of the CAT 390F excavator, I can see that the length from the center to the very back (the tail swing radius) is 4.70 meters, and the height from the ground up to the counterweight is 1.64 meters. I insert the canvas and then scale it to match the known measurements (just divide by 14, since the scale is 1/14).

After the canvas has been scaled properly, I can draw in the 3D-objects on top of it to match the sizes of the features.

Insert canvas:

http://d1t2tbzmskak12.cloudfront.net...or/Canvas1.jpg

http://d1t2tbzmskak12.cloudfront.net...or/Canvas2.jpg

Switch to orthograpic view:

http://d1t2tbzmskak12.cloudfront.net...or/Canvas3.jpg

Draw in the objects:

http://d1t2tbzmskak12.cloudfront.net...or/Canvas4.jpg

Switch back to perspective view:

http://d1t2tbzmskak12.cloudfront.net...or/Canvas5.jpg

Hide canvas:

http://d1t2tbzmskak12.cloudfront.net...or/Canvas6.jpg

Voilà!

Best regards,
Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

On this site you can find specs for most types of construction equipment:
- http://www.ritchiespecs.com/

For example, you can find the specs for this model, the CAT 390F:
- http://www.ritchiespecs.com/specific...modelid=108962

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

The angle sensors are now in place and I've added the code in the motor controllers to:
- Read the angle sensors
- Set a max movement limit
- Set a min movement limit
- Set a customizable hysteresis to avoid rapid on/off switching at the end stops if the sensor fluctuates.
- A way to read the current position from the host CPU (the Raspberry pi).

I also added acceleration and variable braking. Both are compatible with the Pololu quik controller. The acceleration was needed to avoid jerkyness when entering or leaving the end limits. The braking helps a lot when holding the arm statically and avoids back-drive of the gear motors.

The end stop logic works as I hoped, and I no longer have to worry about crashing into the end of the travel.

Just for fun, I added a visualization of the excavator on a web page. It's just the same 3D model from Fusion 360, but exported to STL to be loadable by three.js in a web page. The web page uses WebSocket to read the current position of the joints from the excavator over WiFi.

Here is a short video:

http://d1t2tbzmskak12.cloudfront.net...nglesensor.jpg

https://youtu.be/Z60gWvpeJ8I

Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

You can try the visualization on your own PC by clicking on this link:
- http://d1t2tbzmskak12.cloudfront.net...ion/index.html

http://d1t2tbzmskak12.cloudfront.net...ualization.jpg

To view the client source code, just click "View source" on the web page.

The web page will try to connect to the excavator using the local IP address 192.168.0.107:8001. If it isn't able to connect, it will just play some random movements. Only computers on my network will be able to connect to my excavator, so you all will see the random movements.

On the excavator (the server in this case) I run these few lines to broadcast the positions to all clients that connect:

Code:

var ws = require("nodejs-websocket")

var server = ws.createServer().listen(8001);

setInterval(function() {

    server.connections.forEach(function (conn) {

        conn.sendText(JSON.stringify({bucket: state.bucket.pos, stick: state.stick.pos }));

    })

}, 40);

That's all there is to it!

Best regards,
Stein :-)

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

This is now the code on the motor controllers, that has the new current limiting, position reporting and position end limits, acceleration and variable braking.

You don't need to understand any of this code to use it. Just:

1. Connect the USB cable to a PC
2. Download the Arduino programmer: https://www.arduino.cc/en/Main/Software
3. Copy and paste the code below into the programming tool
4. Set the device ID on the second line (any number from 0 to 255, used to identify the controller later) I'm using device IDs 10, 11 and 12 for this build.
5. Click the "Upload" button

Done!

Re: 90 ton 1/14 metal excavator scratch build w/embedded PC

Code:





/*  Based on the MonsterMoto Shield Example Sketch by: Jim Lindblom */

#include <limits.h>

int deviceId = 10; // TODO: Set this to a unique ID for each controller!





#define BRAKEVCC 0

#define CW   1

#define CCW  2

#define BRAKEGND 3



// VNH2SP30 pin definitions

int inApin[2] = {7, 4};  // INA: Clockwise input

int inBpin[2] = {8, 9}; // INB: Counter-clockwise input

int pwmpin[2] = {5, 6}; // PWM input

int cspin[2] = {2, 3}; // CS: Current sense ANALOG input

int enpin[2] = {0, 1}; // EN: Status of switches output (Analog pin)



// Position sensors

int pospin[2] = {4, 5}; // Position sensors at ANALOG input 4,5



// Serial state

int inBytes[15];

int inBytesCount = 0;

int command = 0;

unsigned long lastCommandTime = 0;

unsigned long timeoutMillis = 1000; 



// Timing

int timerDivisor = 8;

int defaultTimerDivisor = 64;

int millisDivisor = defaultTimerDivisor / timerDivisor;



// Motor state

int current[2] = {0, 0};

int currentLimit[2] = { 160, 160 };

int overcurrentDivisor[2] = {8, 8};

unsigned long overCurrentTime[2] = {0, 0};

int direction[2] = { BRAKEGND, BRAKEGND };

int directionWanted[2] = { BRAKEGND, BRAKEGND };

int speed[2] = { 0, 0 };

int speedPrev[2] = { 0, 0 };

int speedWanted[2] = { 0, 0 };

int acceleration[2] = { 100, 100 };

int position[2] = { 511, 511 };

int positionMin[2] = { INT_MIN, INT_MIN };

int positionMax[2] = { INT_MAX, INT_MAX };

bool positionMaxTriggered[2] = { false, false };

bool positionMinTriggered[2] = { false, false };

int positionHysteresis[2] = { 100, 100 };



// Loop state

unsigned long lastTime = 0;

unsigned long now = 0;

int speedScaler = 4;





void setup() {

  setPwmFrequency(5, timerDivisor);

  setPwmFrequency(6, timerDivisor);

  

  // Initialize digital pins as outputs

  for (int i=0; i<2; i++) {

    pinMode(inApin[i], OUTPUT);

    pinMode(inBpin[i], OUTPUT);

    pinMode(pwmpin[i], OUTPUT);

  }

  // Initialize motors braked

  motorGo(0, BRAKEGND, 255);

  motorGo(1, BRAKEGND, 255);

  

  // start serial port at 9600 bps:

  Serial.begin(9600);

  while (!Serial) {

    // wait for serial port to connect. Needed for native USB port only

  }

}





void loop() {



  // Keep time

  now = millis() / millisDivisor; // to account for the change of the timer by setPwmFrequency below



  // Read commands from serial port

  if (Serial.available() > 0) {

    int inByte = Serial.read();

    if (inByte > 127) {

      inBytesCount = 0;

      command = inByte;

      switch(command) {

        // 0x90 & 0x91: Get Motor M0 & M1 Current

        case 0x90: Serial.write(current[0] / 4); command = 0; break;

        case 0x91: Serial.write(current[1] / 4); command = 0; break;



        // 0x94 & 0x95: Get Motor M0 & M1 position

        case 0x94: Serial.write(position[0] / 4); command = 0; break;

        case 0x95: Serial.write(position[1] / 4); command = 0; break;

      }

      lastCommandTime = now;

    }

    else if (command != 0) {

      if (inBytesCount < 14) {

        inBytes[inBytesCount] = inByte;

      }

      inBytesCount++;

      switch(inBytesCount) {

        case 1: {

          int inSpeed = inByte * 2; // From 0-127 range to 0-254 range

          int motor = -1;

          int inDirection = CW;

          switch(command) {

            case 0x86: motor = 0; inDirection = BRAKEGND; break;

            case 0x87: motor = 1; inDirection = BRAKEGND; break;

            case 0x88: motor = 0; inDirection = CW; break;

            case 0x8A: motor = 0; inDirection = CCW; break;

            case 0x8C: motor = 1; inDirection = CW; break;

            case 0x8E: motor = 1; inDirection = CCW; break;

            case 0x83: {

              if (inBytes[0] == 0) {

                Serial.write(deviceId);

              }

              command = 0;

              break;

            }

          }

          if (motor != -1) {

            speedWanted[motor] = inSpeed;

            directionWanted[motor] = inDirection;

            command = 0;

          }

          break;

        }

        case 4: {

          // 0x84, parameter number, parameter value, 0x55, 0x2A // Set parameter

          // Parameter number 8 and 9 are the current limits for motors 0 and 1 respectively

          switch(command) {

            case 0x84: {

              int retVal = 1;

              if (inBytes[2] == 0x55 && inBytes[3] == 0x2A) {

                retVal = 0;

                switch(inBytes[0]) {

                  case 4: acceleration[0] = inBytes[1]; break; // 0-127

                  case 5: acceleration[1] = inBytes[1]; break; // 0-127 

                  case 8: currentLimit[0] = inBytes[1] * 8; break; // 0-1016

                  case 9: currentLimit[1] = inBytes[1] * 8; break; // 0-1016 

                  case 72: positionMax[0] = inBytes[1] * 8; break; // 0-1016

                  case 73: positionMax[1] = inBytes[1] * 8; break; // 0-1016

                  case 74: positionMin[0] = inBytes[1] * 8; break; // 0-1016

                  case 75: positionMin[1] = inBytes[1] * 8; break; // 0-1016

                  case 76: positionHysteresis[0] = inBytes[1] * 8; break; // 0-1016

                  case 77: positionHysteresis[1] = inBytes[1] * 8; break; // 0-1016

                  default: retVal = 2; break;

                }

              }

              Serial.write(retVal);

              command = 0;

              break;

            }

          }

          break; 

        }

      }

    }

  }







  // Update motor state every 10ms

  if (lastTime == 0 || now < lastTime) { // If the first time or when the millis() values wrap, we need to fix the lastTime to be before the now time.

    lastTime = now;

  }

  if (now - lastTime >= 10) {

    lastTime = now;

      

    for (int i = 0; i < 2; i++) {

      bool sameDirection = direction[i] == directionWanted[i];

      direction[i] = directionWanted[i];

      speed[i] = speedWanted[i];

      

      if ((direction[i] == CW) || (direction[i] == CCW)) {

  

        // Apply accelleration limiting

        int accelSpeed = speedScaler * speed[i]; // 10ms loop time and *4 gives the 40ms as in the Pololu controller

        if (direction[i] == CCW) {

          accelSpeed = -accelSpeed;

        }

        if ((acceleration[i] > 0) &&

            (!(sameDirection && (abs(accelSpeed) < abs(speedPrev[i])))) &&

            (abs(accelSpeed - speedPrev[i]) > acceleration[i])) {

          accelSpeed = speedPrev[i] + ((speedPrev[i] > accelSpeed) ? -acceleration[i] : acceleration[i]);

        }

        direction[i] = accelSpeed < 0 ? CCW : CW;

        speed[i] = abs(accelSpeed / speedScaler);

        speedPrev[i] = accelSpeed;

  

        // Apply current limiting

        current[i] = analogRead(cspin[i]);

        // If overcurrent, kill the output

        if (current[i] > currentLimit[i]) {

          overcurrentDivisor[i] = 0;

          overCurrentTime[i] = now;

        }

        // Slowly bring it back

        if (now > overCurrentTime[i] + 1000 && overcurrentDivisor[i] < 8) {

          overcurrentDivisor[i]++;

          overCurrentTime[i] = now;

        }

        speed[i] = speed[i] * overcurrentDivisor[i] / 8;

        

      }

    

      // Apply position limits

      int readPosition = analogRead(pospin[i]);

      int changeLimit = positionHysteresis[i] / 2 + 10;

      if (abs(readPosition - position[i]) > changeLimit) {

        position[i] = position[i] + (position[i] > readPosition ? -changeLimit : changeLimit);

      } else {

        position[i] = readPosition;

      }    

      if (position[i] > positionMax[i]) {

        positionMaxTriggered[i] = true;

      }

      if (position[i] < positionMax[i] - positionHysteresis[i]) {

        positionMaxTriggered[i] = false;

      }

      if (position[i] < positionMin[i]) {

        positionMinTriggered[i] = true;

      }

      if (position[i] > positionMin[i] + positionHysteresis[i]) {

        positionMinTriggered[i] = false;

      }

      if ((positionMaxTriggered[i] && direction[i] == CW) ||

          (positionMinTriggered[i] && direction[i] == CCW)) {

        direction[i] = BRAKEGND;

        speed[i] = 255;

        speedPrev[i] = 0;

      }

  

      // Stop on serial command timeout

      if (now - lastCommandTime > timeoutMillis) {

        direction[i] = BRAKEGND;

        speed[i] = 0;

        speedPrev[i] = 0;

      }

    

      motorGo(i, direction[i], speed[i]);

    }

  }

  

}







void motorGo(uint8_t motor, uint8_t direct, uint8_t pwm) {

  if (motor >= 0 && motor < 2 && direct >= 0 && direct < 4 && pwm >= 0 && pwm < 256 ) {

    digitalWrite(inApin[motor], (direct == BRAKEVCC) || (direct == CW) ? HIGH : LOW);

    digitalWrite(inBpin[motor], (direct == BRAKEVCC) || (direct == CCW) ? HIGH : LOW);

    analogWrite(pwmpin[motor], pwm);

  }

}







void setPwmFrequency(int pin, int divisor) {

  byte mode;

  if(pin == 5 || pin == 6 || pin == 9 || pin == 10) {

    switch(divisor) {

      case 1: mode = 0x01; break;

      case 8: mode = 0x02; break;

      case 64: mode = 0x03; break;

      case 256: mode = 0x04; break;

      case 1024: mode = 0x05; break;

      default: return;

    }

    if(pin == 5 || pin == 6) {

      TCCR0B = TCCR0B & 0b11111000 | mode;

    } else {

      TCCR1B = TCCR1B & 0b11111000 | mode;

    }

  } else if(pin == 3 || pin == 11) {

    switch(divisor) {

      case 1: mode = 0x01; break;

      case 8: mode = 0x02; break;

      case 32: mode = 0x03; break;

      case 64: mode = 0x04; break;

      case 128: mode = 0x05; break;

      case 256: mode = 0x06; break;

      case 1024: mode = 0x7; break;

      default: return;

    }

    TCCR2B = TCCR2B & 0b11111000 | mode;

  }

}