# Exoskeleton Motor Control Code

Background:

I have been working on an exoskeleton project with a group of friends, and I need help. I made another post on the robotics.stackexchange site for the drive system: https://robotics.stackexchange.com/questions/4323/exoskeleton-drive-system-help. You can also check out our first prototype on youtube: https://www.youtube.com/watch?v=NL_aCwJSRiE&feature=youtu.be

The device is going to be controlled by an iPad that we created via bluetooth. We are using the adafruit bluefruitLE

We are attempting to control 4 motors (2 knee motors and 2 hip motors) from an Arduino Mega. We will be using 2 Roboclaw 2x60A motor controllers. We are trying to get the motors to mimic the graph (see attached).

The graph is in degrees per percent cycle (a gait cycle). We are using 50 gait cycles per minute for tests but we hope to make it adjustable.
We can change percent cycle to time in milliseconds using dimensional analysis (ie) 50 cycles/min = 8.333x10^-4 cycles/ms —-> 1/8.333x10^-4 cycles/ms is approx. 1200 ms/cycle. So if you multiply the cycle times 1200 you get millisecond for a certain percent of the cycle (ie) 0.5cycles(50 percent of cycle) * 1200ms/cycle = 600 ms.

We will have a position feedback system incorporated in the drive system but because it is a prototype and I wanted to make the code as versatile as posible I created a function: getDegrees(int jointNum), so if I change the position feedback system I only have to change the getDegrees method.

In order to mimic the graph we broke the graph up into sections. Each section represents a different velocity. We are going to run a correction algorithm so far we have just done the P in PID, we have used a modifier of 1.5. Then we saved the value that the section ended on so next gait cycle when it runs that section the algorithm will start off closer to the target value.

My question:

I need help developing a strategy in order to sync the movement of the 4 joints while running a correction algorithm on each joint individually. While the device is running we need to be able to constantly check for a bluetooth message from the iPad. The bluetooth messages come in via SPI from the adafruit bluefruitLE.

What I have done so far:

I have been able to run a single joint by using a while statement (ie)

``````int i = 0;
void loop(){
targetAngle = nextAngle[i]; //nextAngle is an array of points so nextAngle[0] through nextAngle[1] represents one section of the graph.
i++
if (getDegrees(1)< targetAngle){
double deg = getDegrees(1);
while (deg<targetAngle){
/*
*
*Execute drive code
*
*/
deg = getDegrees(1);
}else{
double deg = getDegrees(1);
while (deg>targetAngle){
/*
*
*Execute drive code
*
*/
deg = getDegrees(1);
}
}
}
``````

The main problem with that code is its like using the delay() function instead of millis() is that it stops the program while its executing the code, and I cannot run multiple motors and check for bluetooth messages.

Major Challenges:

The drive commands must be based off of angle and not time. It is ok if we tell it do 50 cycles per minute and it does 52 or 48 but its not okay if it only does 97% of the motion.

Possible solutions I have come up:

Using one arduino for each joint, and making them all slaves of a master arduino and the master arduino would act as the middle man sort of between the joints, making sure that they are synced up. That way each arduino could use code similar to what I have already tried and confirmed to work. But I would prefer to run it all from one arduino.

Conclusion:

I need help developing a strategy in order to sync the movement of the 4 joints while running a correction algorithm on each joint individually.

Is this the right forum for this question? Or is it more of a Stack Overflow or Robotics question?

Any help will be greatly appreciated,

--  Joel

• Writing an answer now and curious: Why do you prefer delay() over millis()? – Anonymous Penguin Jul 30 '14 at 1:41
• I dont prefer delay() over millis(). I was using that as an analogy to show that while loops stop the void loop() function from continuing to loop like the delay() function does. I prefer millis() over delay() because it doesn't stop the void loop() function. – Joel Jul 30 '14 at 5:13

Here's what I came up with:

• You have a certain percent of the gain cycle equal one "step" (step may be a bad term in this situation, but I'm using it because of stepper motors).
• You have a blocking function that will move the motors and control all the P(ID) logic
• Before this function is called, the main code will use a variable to store the desired time to do the next "step."
• It will have a loop with "substeps." So it would look kinda like this:
``````void function nextStep(int var1, int var2) {
//Read sensors and put into variables
until(condition == true) { //Condition to see if the motors are at the right degree
//Adjust motors not in the right place
delayAndCheckBT(10); //Look at the bottom for this function
}
}
``````

You could also have it accept a few percent of variance as tolerance so it's not shakey, but that's more of the PID control than anything. You'll have to make sure that many 'steps' doesn't make the PID controller unstable.

• When that code is done, the main function will check to see if the desired time for the next step has passed (or if it's now). If it's passed/now, then it will execute. Otherwise, it will not execute the next step and continue executing the main loop.

Great? No. The problem is you need to check for bluetooth! To do this, I've made two functions:

``````void checkBT() {
//Checks BT. Make sure this is non blocking: if you need to, set a few global variables
//Note this code will get called a LOT, so don't implement any delays or loops: inside here: use millis().
}
void delayAndCheckBT(int timeToWait) {
unsigned long finishTime = millis() + timeToWait;
while (millis()<finishTime) {
checkBT();
}
}
``````

As your question "to sync the movement of the 4 joints while running a correction algorithm on each joint individually."

Referring to the first part of your question, sync the movement, assume your are doing this at either commercial or university project level, popular practice is to use 'hard real time preemptive operating system".

RTOS, real time operating system, is a piece of system software that coordinates execution of your multi thread user software at precise and predictable time accuracy in order of millisecond.

RTOS involve money and learning time. You may want to continue using the simpler system that you already having now and work in just one joint, hence, no issue of syncing multiple joints. Once you reach some maturity on single joint, then, expand to syncing 4 joints.

RTOS can be commercial with full support or free of charge. Some are dual mode. Same software may either be free or if you choose, can buy commercial full support. There are simple and full-scale implementations where functionality, complexity, learning time and software cost (commercial or free) varies by huge extend. You will then be more able to choose when you done the above single joint system.

Introduction http://en.wikipedia.org/wiki/RTOS

There are also real time 'executive' (smaller and less functions). For example, ucos II (older and simple version) is used by many people.

Hope it helps

• Thanks for the response, it got me to think more about how to order the processing. What I am thinking now is to maybe stack the knee and hip graph and cut them up into equal sections. Then have one angle as a reference and run a while loop based on that angle and run the drive commands for the other joints inside that while loop. Theoretically when the reference joint gets to its target position the other joints should get there as well, however theoretically is not reality so inside the while loop I will add if statements to make sure the joints don't over rotate. – Joel Jul 30 '14 at 17:09
• then after several cycles the algorithm will have hopefully honed in on the target velocity and the other joints will finish closer and closer to the target position. – Joel Jul 30 '14 at 17:12
• Does that make any sense? Do you think that would work? – Joel Jul 30 '14 at 17:12
• Human and robot motion is a table of time and many independent TARGET angles at many joints.Example, at time 0,angle0 is 0,angle1 is 10; at time 0.01 second,angle0 is 10,angle1 is 30; at time 0.02sec,angle0 30,angle1 60 Then, feedback_error=actualangle-TAGET_ANGLE Use PID to control Motor based on error. Actual motion is a table of time and corresponding angles. 100 to 300 rows should be enough. One second 10 to 30 steps for smooth motion. Video human and play frame by frame to see, video is 25 to 30 frames per second. – EEd Jul 30 '14 at 17:42
• These 'toys' robot comes with pc software for user to enter the 'time table' and the robot will move and dance. Typical has 10 to 20 joints. Comes with pc software for user to enter table and can selectively 'play back' from time point a to b. dx.com/p/… robosavvy.com/store/images/khr3hvstore.jpg youtube.com/watch?v=d3O2dQQTdYM – EEd Jul 30 '14 at 17:48