File content as of revision 1:709be64ca63c:

#include "mbed.h"
#include "RobotServo.h"

PwmOut pwm_lw(D2);   //left wrist
PwmOut pwm_le(D3);   //left elbow
PwmOut pwm_ls(D4);   //left shoulder
PwmOut pwm_la(D5);   //left ankle
PwmOut pwm_lk(D6);   //left knee
PwmOut pwm_ll(D7);   //left leg
PwmOut pwm_lb(D8);   //left butt
PwmOut pwm_rb(D10);  //right butt
PwmOut pwm_rl(PA_11);//right leg
PwmOut pwm_rk(D13);  //right knee
PwmOut pwm_ra(D14);  //right ankle
PwmOut pwm_rs(D15);  //right shoulder
PwmOut pwm_re(PC_9); //right elbow
PwmOut pwm_rw(PC_8); //right wrist

const int servoangle_left[TOT_ARM_POS][3] = {   {-7,0,-25},{-5,0,-30},{-6,0,-28},{-7,0,-26},{-27,28,-33},{-36,39,-32},{-44,46,-30},{-50,52,-28},{-56,57,-26},{-61,62,-24},
                                        {-66,65,-21},{-70,68,-19},{-75,71,-17},{-79,74,-14},{-83,76,-12},{-87,78,-10},{-90,80,-7},{-94,81,-5},{-97,83,-2},{-101,84,0},
                                        {-104,85,3},{-107,86,5},{-110,87,7},{-113,87,10},{-115,88,12},{-118,88,14},{-121,89,16},{-123,89,18},{-125,89,20},{-127,89,22},
                                        {-130,89,23},{-131,89,25},{-133,88,26},{-135,88,27},{-136,88,28},{-137,87,29},{-138,87,30},{-139,86,30},{-140,86,30},{-140,85,29},{-140,84,28}};
const int servoangle_right[TOT_ARM_POS][3] = {{140,-84,-28},{140,-85,-29},{140,-86,-30},{139,-86,-30},{138,-87,-30},{137,-87,-29},{136,-88,-28},{135,-88,-27},{133,-88,-26},
                                        {131,-89,-25},{130,-89,-23},{127,-89,-22},{125,-89,-20},{123,-89,-18},{121,-89,-16},{118,-88,-14},{115,-88,-12},{113,-87,-10},{110,-87,-7},
                                        {107,-86,-5},{104,-85,-3},{101,-84,0},{97,-83,2},{94,-81,5},{90,-80,7},{87,-78,10},{83,-76,12},{79,-74,14},{75,-71,17},{70,-68,19},
                                        {66,-65,21},{61,-62,24},{56,-57,26},{50,-52,28},{44,-46,30},{36,-39,32},{27,-28,33},{7,0,26},{6,0,28},{5,0,30},{7,0,25}};

const float footangle_left[TOT_FOOT_POS][4]= {
    {9.24   ,115.5  ,-98    ,-6},       //0
    {9.24   ,115.5  ,-95.5  ,-9},       //1
    {9.24   ,116    ,-93    ,-11},      //2
    {9.25   ,116    ,-90.2  ,-11},      //3
    {9.27   ,115    ,-84.98 ,-14.13},   //4
    {9.065  ,115    ,-83.38 ,-13.5},    //5
    {8.86   ,115    ,-80    ,-12.739},  //6
    {8.86   ,115    ,-80    ,-12.739},  //s1 for stop mode (from 6)
    {8.54 ,113  ,-78.89 ,-11.36}, //s2 for stop mode (from 7)
    {8.22  ,113  ,-78  ,-6},   //s3 for stop mode (from 8)
    {8.22  ,106  ,-73  ,-5},   //s4 for stop mode (from 9)
    {8.22  ,101  ,-69  ,-2},   //s5 for stop mode (from 10)
    {7.99  ,100  ,-64  ,0.2},    //s6 for stop mode (from 11)
    {7.99  ,96   ,-61  ,3},    //s7 for stop mode (from 12)
    {8.54   ,113    ,-78.89 ,-11.36},   //7
    {8.22   ,113    ,-78    ,-6},       //8
    {8.22   ,106    ,-73    ,-5},       //9
    {8.22   ,101    ,-69    ,-2},       //10
    {7.99   ,100    ,-64    ,0.2},      //11
    {7.99   ,96     ,-61    ,3},        //12
    {7.99   ,92     ,-59.5  ,6},        //13
    {7.86   ,89     ,-57.04 ,8.5},      //14
    {7.73   ,87     ,-56    ,10},       //15
    {7.63   ,84     ,-56    ,12},       //16
    {7.53   ,82     ,-55    ,14},       //17
    {5.77   ,80     ,-53    ,15},       //18
    {4.01   ,74     ,-53    ,16},       //19
    {4.485  ,70     ,-52    ,18.5},     //20
    {4.96   ,64     ,-45.89 ,14.6},     //21
    {5.44   ,62     ,-43.57 ,14},       //22
    {5.92   ,58     ,-44.25 ,14.52},    //23
    {5.92   ,56     ,-43.485,15.5},     //24
    {5.91   ,54     ,-44.72 ,16.5},     //25
    {5.91   ,54.45  ,-45    ,16},       //26
    {5.91   ,56.4   ,-51.5  ,13.53},    //27
    {5.91   ,57     ,-55.345,13.5},     //28
    {5.91   ,57     ,-57    ,13},       //29
    {5.92   ,58     ,-59.09 ,12.94},    //30
    {5.94   ,58     ,-60    ,11.5},     //31
    {5.95   ,59     ,-62    ,10.5},     //32
    {5.96   ,60     ,-65    ,11},       //33
    {5.97   ,62.5   ,-69    ,11.75},    //34
    {5.98   ,65.6   ,-75.77 ,12},       //35
    {6.07   ,70.7   ,-83    ,15.5},     //36
    {6.16   ,75.87  ,-92.37 ,20},       //37
    {6.22   ,83.07  ,-105   ,28},       //38
    {6.51   ,85.53  ,-105.5 ,24},       //39
    {6.51   ,89.03  ,-106   ,22},       //40
    {12.17  ,93.9   ,-106.84,16.5},     //41
    {12     ,100    ,-108.5 ,13},       //42
    {12.6   ,108    ,-110   ,10},       //43
    {12.25  ,112    ,-109.5 ,6.5},      //44
    {12     ,114    ,-108.87,4},        //45
    {12.45  ,115    ,-106.1 ,2},        //46
    {8.9    ,115.5  ,-101.5 ,-3.5}};    //47
                               
const float footangle_right[TOT_FOOT_POS][4] = {
    {-5.92  ,-63    ,42     ,-4.5},     //0
    {-5.91  ,-62.5  ,44     ,-6},       //1
    {-5.91  ,-61.5  ,45     ,-7},       //2
    {-5.91  ,-60    ,47.95  ,-8},       //3
    {-5.91  ,-61.5  ,52.19  ,-11},      //4
    {-6.01  ,-62.04 ,58.71  ,-14.265},  //5
    {-6.11  ,-65.68 ,65.24  ,-15.15},   //6
    {-6.11  ,-46.68 ,30.24  ,14.15},    //(-6.11    ,-65.68^^^  ,65.24vvv   ,-15.15^^^)s1 used for stop mode (from 6)
    {-6.2   ,-45.51 ,30.1   ,15},     //(-6.2 ,-69.51^^^ ,75.1vvv ,-20^^^)s2 used for stop mode (from 7)
    {-6.47  ,-45.34 ,30.5 ,18.12},   //(-6.47 ,-73.34 ,84.5 ,-23.12)s3 used for stop mode (from 8)
    {-6.47  ,-45.29 ,30.5 ,24.79},  //(-6.47  ,-75.29 ,86.5 ,-20.79)s4 used for stop mode (from 9)
    {-6.47  ,-42.245,25   ,30.475},  //(-6.47  ,-77.245,88   ,-15.475)s5 used for stop mode (from 10)
    {-6.61  ,-37.15 ,17.56  ,41.7},  //(-6.61  ,-81.15 ,92.56  ,-15.7)s6 used for stop mode (from 11)
    {-6.61  ,-34.65 ,11.41  ,49.492},  //(-6.61  ,-83.65 ,96.41  ,-16.492)s7 used for stop mode (from 12)
    {-6.2 ,-69.51   ,75.1   ,-20},      //7
    {-6.47  ,-73.34 ,84.5   ,-23.12},   //8
    {-6.47  ,-75.29 ,86.5   ,-20.79},   //9
    {-6.47  ,-77.245,88     ,-15.475},  //10
    {-6.61  ,-81.15 ,92.56  ,-15.7},    //11
    {-6.61  ,-83.65 ,96.41  ,-16.492},  //12
    {-6.61  ,-86.15 ,97.5   ,-13.67},   //13
    {-6.7   ,-88.23 ,99     ,-13},      //14
    {-6.79  ,-90.31 ,98     ,-7},       //15
    {-6.83  ,-91    ,97.5   ,-3},       //16
    {-6.88  ,-92.395,95.035 ,2.5},      //17
    {-7.58  ,-94.28 ,92.84  ,9.435},    //18
    {-8.28  ,-96.17 ,88.66  ,17.54},    //19
    {-8.66  ,-98.52 ,84.26  ,25.07},    //20
    {-9.05  ,-102.88,83.73  ,28.61},    //21
    {-9.09  ,-100.88,78.3   ,33.49},    //22
    {-9.14  ,-101.75,72.9   ,39.57},    //23
    {-9.19  ,-101.75,69.05  ,41.25},    //24
    {-9.24  ,-102.5 ,67.2   ,43.14},    //25
    {-9.25  ,-104   ,67     ,41.13},    //26
    {-9.26  ,-108   ,64.59  ,39.35},    //27
    {-9.26  ,-109.5 ,65.59  ,37.27},    //28
    {-9.27  ,-110   ,66.59  ,31.41},    //29
    {-9.2   ,-110   ,66.59  ,28.55},    //30
    {-9.2   ,-112   ,69.4   ,21.7},     //31
    {-9.16  ,-111   ,69.7   ,18.48},    //32
    {-9.13  ,-111   ,68.5   ,15.27},    //33
    {-8.75  ,-106.17,62.54  ,18.52},    //34
    {-8.65  ,-103   ,60.3   ,16.68},    //35
    {-8.02  ,-101.3 ,60.85  ,12.25},    //36
    {-7.76  ,-100.64,63.41  ,5.18},     //37
    {-7.64  ,-97.59 ,62.07  ,1.61},     //38
    {-7.53  ,-94.54 ,60.74  ,-1.04},    //39
    {-7.42  ,-92.74 ,60.1   ,-3.15},    //40
    {-7.315 ,-89.8  ,63.4   ,-9.275},   //41
    {-6.41  ,-84.63 ,63.1   ,-14.99},   //42
    {-6.35  ,-82.51 ,62     ,-17.5},    //43
    {-6.24  ,-78.07 ,62     ,-18.8},    //44
    {-6.19  ,-72.945,54     ,-10.77},   //45
    {-6.14  ,-68    ,46     ,-5.7},     //46
    {-6.09  ,-64.985,41     ,-4.5}};    //47
                                
extern void setupServo(void){
    pwm_lw.period(PWM_PERIOD);pwm_lw.pulsewidth(POWER_REDU_PW);
    pwm_le.period(PWM_PERIOD);pwm_le.pulsewidth(POWER_REDU_PW);
    pwm_ls.period(PWM_PERIOD);pwm_ls.pulsewidth(POWER_REDU_PW);
    pwm_la.period(PWM_PERIOD);pwm_la.pulsewidth(POWER_REDU_PW);
    pwm_lk.period(PWM_PERIOD);pwm_lk.pulsewidth(POWER_REDU_PW);
    pwm_ll.period(PWM_PERIOD);pwm_ll.pulsewidth(POWER_REDU_PW);
    pwm_lb.period(PWM_PERIOD);pwm_lb.pulsewidth(POWER_REDU_PW);
    pwm_rb.period(PWM_PERIOD);pwm_rb.pulsewidth(POWER_REDU_PW);
    pwm_rl.period(PWM_PERIOD);pwm_rl.pulsewidth(POWER_REDU_PW);
    pwm_rk.period(PWM_PERIOD);pwm_rk.pulsewidth(POWER_REDU_PW);
    pwm_ra.period(PWM_PERIOD);pwm_ra.pulsewidth(POWER_REDU_PW);
    pwm_rs.period(PWM_PERIOD);pwm_rs.pulsewidth(POWER_REDU_PW);
    pwm_re.period(PWM_PERIOD);pwm_re.pulsewidth(POWER_REDU_PW);
    pwm_rw.period(PWM_PERIOD);pwm_rw.pulsewidth(POWER_REDU_PW);
}

void pwm_left(float angle_up, float angle_mid, float angle_down){   //Left arm
  pwm_ls.pulsewidth((angle_up+10.0f) / 90.0f * 0.0008f + 0.002297f);
  pwm_le.pulsewidth(angle_mid / 90.0f * 0.0008f + 0.0007044f);
  pwm_lw.pulsewidth(-angle_down / 90.0f * 0.0008f + 0.001505f);
//  OCR3B = 574.0+(400.0/180.0)*(angle_up+10); //pin 2
//  OCR2B = 44.0+(100.0/180.0)*angle_mid; //pin 9
//  OCR0B = 94+(100.0/180.0)*(-1)*angle_down; // pin 4
}
void pwm_right(float angle_up, float angle_mid, float angle_down){  //Right arm
  pwm_rs.pulsewidth((angle_up-3.0f) / 90.0f * 0.0008f + 0.0007f);
  pwm_re.pulsewidth(angle_mid / 90.0f * 0.0008f + 0.0023f);
  pwm_rw.pulsewidth(-angle_down / 90.0f * 0.0008f + 0.001505f);
//  OCR1B = 175+(400.0/180.0)*(angle_up-3); //pin 12
//  OCR2A = 144.0+(100.0/180.0)*angle_mid; //pin 10
//  OCR0A = 94+(100.0/180.0)*(-1)*angle_down; // pin 13
}
void lookuptable_steering(int8_t sensor_output){
    int i = sensor_output/2;
    int q1 = servoangle_left[20+i][0];
    int q2 = servoangle_left[21+i][0];
    int q3 = servoangle_left[20+i][1];
    int q4 = servoangle_left[21+i][1];
    int q5 = servoangle_left[20+i][2];
    int q6 = servoangle_left[21+i][2];
    int q7 = servoangle_right[20+i][0];
    int q8 = servoangle_right[21+i][0];
    int q9 = servoangle_right[20+i][1];
    int q10 = servoangle_right[21+i][1];
    int q11 = servoangle_right[20+i][2];
    int q12 = servoangle_right[21+i][2];
    float x_L1 = (q2-q1)*(sensor_output-2*i)/2 + q1;
    float x_L2 = (q4-q3)*(sensor_output-2*i)/2 + q3;
    float x_L3 = (q6-q5)*(sensor_output-2*i)/2 + q5;
    float x_R1 = (q8-q7)*(sensor_output-2*i)/2 + q7;
    float x_R2 = (q10-q9)*(sensor_output-2*i)/2 + q9;
    float x_R3 = (q12-q11)*(sensor_output-2*i)/2 + q11;
    pwm_left(x_L1,x_L2,x_L3);
    pwm_right(x_R1,x_R2,x_R3);
}

void pwm_left_foot(float angle_1,float angle_2,float angle_3,float angle_4){
  pwm_lb.pulsewidth(POWER_REDU_PW);
  pwm_ll.pulsewidth(angle_2 / 90.0f * 0.0008f + 0.0007f);
  pwm_lk.pulsewidth(angle_3 / 90.0f * 0.0008f + 0.002297f);
  pwm_la.pulsewidth(-angle_4 / 90.0f * 0.0008f + 0.001513f);
//  OCR4A = 25;//(-(angle_1 + 4.0) / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 6
//  OCR3A = 175 + (400/180)*angle_2; // Pin 5
//  OCR3C = 574 + (400/180)*angle_3; // Pin 3
//  OCR5B = 378 + (400/180)*angle_4*(-1); // Pin 45
}
void pwm_right_foot(float angle_1,float angle_2,float angle_3,float angle_4){
  pwm_rb.pulsewidth(8.5f / 90.0f * 0.0008f + 0.0015f);
  pwm_rb.pulsewidth(angle_2 / 90.0f * 0.0008f + 0.002297f);
  pwm_rb.pulsewidth(angle_3 / 90.0f * 0.0008f + 0.0007f);
  pwm_rb.pulsewidth(-angle_4 / 90.0f * 0.0008f + 0.001513f);
//  OCR4B = (8.5 / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 7
//  OCR4C = 574 + (400/180)*angle_2; // Pin 8
//  OCR1A = 175 + (400/180)*angle_3; // Pin 11
//  OCR5A = 378 + (400/180)*angle_4*(-1); // Pin 46
}
void lookuptable_pedaling(int index){
  int q0 = footangle_left[index][0];
  int q1 = footangle_left[index][1];
  int q2 = footangle_left[index][2];
  int q3 = footangle_left[index][3];
  int q4 = footangle_right[index][0];
  int q5 = footangle_right[index][1];
  int q6 = footangle_right[index][2];
  int q7 = footangle_right[index][3];
  pwm_left_foot(q0,q1,q2,q3);
  pwm_right_foot(q4,q5,q6,q7);
}
void reset_left_butt(void)
{
  pwm_lb.pulsewidth(-12.0f / 90.0f * 0.0008f + 0.0015f);
//  OCR4A = (-12.0 / 90.0 * 0.0008 + 0.0015) * 244 * 1024; // Pin 6, left butt
}
void reset_pos(void)
{
  pwm_left_foot(8.86  ,115  ,-80  ,-12.739);
  reset_left_butt();
  pwm_right_foot(-6.11 ,-48.68 ,32.24  ,12.15);
//  pwm_right_foot(-6.11 ,-50.68 ,38.24  ,4.15);
}
void stop_pos(void)
{
  pwm_left_foot(8.86  ,115  ,-80  ,-12.739);
  reset_left_butt();
//  pwm_right_foot(-6.11 ,-48.68 ,32.24  ,12.15);
  pwm_right_foot(-6.11 ,-50.68 ,38.24  ,4.15);
}
int angle_to_duty(int resolution, float degree)
{//+-90 degrees
  return (int)((degree / 90.0f * 0.0008f + 0.0015f) * 244.0f * resolution);
}