Exoskeleton Motor Control Code

Question

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

Answer 1

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

Answer 2

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();
    }
}