P2 SCARA does tower

iseries

I converted my P1 SCARA code over to the P2.
Had to make a couple of changes to get the code to work since smart pins were needed and cog_run was different on the P2.

I also used my P2Ex expansion plugs to work with my Plug and Play modules.

I have a video of it moving the SCARA about picking up pieces that are 5mm thick.

Here is the C code that move the SCARA:

#include <stdio.h>
#include <propeller.h>
#include "JSON.h"
#include "step.h"
#include "servos.h"


void doClaw(void *);
void setAngles(int, int, int);
void Claw(int);
void OpenClose(int);


#define ENABLEA 8
#define STEPA 9
#define DIRECTIONA 10
#define ENABLEB 11
#define STEPB 12
#define DIRECTIONB 13
#define ENABLEC 16
#define STEPC 17
#define DIRECTIONC 18
#define SERVO1 14
#define SERVO2 15

int Steps_s, Steps_e, Steps_h;
int ElbowOffset;
volatile int CW;

char Buffer[256];
char *x;
int Motor_s, Motor_e, Motor_h;
int Stack[50];


int main(int argc, char**argv)
{
    int i, j;
    
  	int e, s, c, h;
  	Motor_s = Step_Start(ENABLEA, DIRECTIONA, STEPA);
  	Motor_e = Step_Start(ENABLEB, DIRECTIONB, STEPB);
  	Motor_h = Step_Start(ENABLEC, DIRECTIONC, STEPC);
  
  	Servo_Enable(SERVO1, 1500, 20000);
    Servo_Enable(SERVO2, 1500, 20000);
    
    _cogstart(doClaw, NULL, Stack, 40);
    
    _waitms(1000);
    
    Claw(0);
    OpenClose(0);
    
    printf("Ready\n");
    
    while (1)
    {
        getStr(Buffer, 100);
    
    	json_init(Buffer);
    	x = json_find("Claw");
    	c = atoi(x);
    	x = json_find("Elbow");
    	e = atoi(x);
    	x = json_find("Height");
    	h = atoi(x);
    	x = json_find("OpenClose");
    	i = atoi(x);
    	x = json_find("Shoulder");
    	s = atoi(x);
    	setAngles(s, e, h);
    	Claw(c);
    	OpenClose(i);
    	Step_Wait();

    	printf(Buffer);
    	printf("\n");
	}
}

void OpenClose(int v)
{
  	if (v)
    	Servo_Set(SERVO2, 1500);
  	else
    	Servo_Set(SERVO2, 1000);
}

void Claw(int a)
{
  	CW = a * 12;
}

void doClaw(void *par)
{
  	int c;
  
  	c = 0;
  	CW = 0;
  
  	while (1)
  	{
    	if (c > CW)
      		c--;
    	if (c < CW)
      		c++;
    
    	Servo_Set(SERVO1, 1550 + c);
    	usleep(1000);
  	}    
}

void setAngles(int s, int e, int h)
{
  	int offset;
  	int sa, ea;
  
  	offset = s * 35;
  	offset = offset - ElbowOffset;
  	ElbowOffset = ElbowOffset + offset;
  	sa = s * 39;
  	sa = sa - Steps_s;
  	Steps_s = Steps_s + sa;
  	ea = e * 64;
  	ea = ea - Steps_e;
  	Steps_e = Steps_e + ea + ElbowOffset;
  	Steps_h = -h * 400;
  	Step_Step(Motor_e, Steps_e);
  	Step_Step(Motor_h, Steps_h);
  	Step_Step(Motor_s, Steps_s);
  	usleep(100);
}

I have created several libraries for devices used on the P2. Custom P2 Libraries

Here is a video of SCARA doing the Tower: P2 SCARA

Mike

AwesomeCronk

Very neat! Really cool to see how precise this thing is!