Skip to content

Commit

Permalink
スピード調整をDelay制御に変更
Browse files Browse the repository at this point in the history
  • Loading branch information
@nnks1010
nnks1010 committed Dec 11, 2015
1 parent 725e0fe commit fc495a1
Show file tree
Hide file tree
Showing 3 changed files with 136 additions and 46 deletions.
22 changes: 5 additions & 17 deletions Design01.cydsn/Servo.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,16 +13,11 @@
#include <stdio.h>
#include "Servo.h"

#define STATE_SEND 0
#define STATE_WAIT 1
#define STATE_GET 2

void Servo_Dataset(Servo_Data* servo, uint8 id, uint8 speed, uint8 stretch, int angle){
servo->id = id;
servo->speed = speed;
servo->stretch = stretch;
servo->angle = angle;
servo->state = STATE_SEND;
}

void EEPROM_rx(Servo_Data* servo){
Expand Down Expand Up @@ -118,21 +113,14 @@ cystatus init_stretch(Servo_Data* servo){
return CYRET_SUCCESS;
}

cystatus init_speed(Servo_Data* servo){
cystatus speed_set(Servo_Data* servo){
uint16 count = 0;
unsigned char tx[3];
tx[0] = 0xC0 | servo->id;
tx[1] = 0x02;
tx[2] = (unsigned char) servo->speed;
UART_servo_PutArray(tx, 3);
while(UART_servo_GetRxBufferSize() < 6){
count += 1;
if(count > 500){
count = 0;
return CYRET_EMPTY;
}
CyDelayUs(1);
}
CyDelay(1);
UART_servo_ClearRxBuffer();
return CYRET_SUCCESS;
}
Expand All @@ -147,7 +135,7 @@ void angle_set(Servo_Data* servo, int16 angle){
tx[1] = (unsigned char)(pos >> 7) & 0x7F;
tx[2] = (unsigned char) pos & 0x7F;
UART_servo_PutArray(tx, 3);
CyDelayUs(500);
CyDelay(1);
}

void angle_get(uint8 id) {
Expand All @@ -161,7 +149,7 @@ void angle_get(uint8 id) {
tx[1] = 0x00;
tx[2] = 0x00;
UART_servo_PutArray(tx, 3);
CyDelayUs(500);
CyDelay(1);
for(i = 0; i < 6; i++){
rx[i] = (unsigned char) UART_servo_GetChar();
}
Expand All @@ -182,7 +170,7 @@ void angle_keep(Servo_Data* servo){
tx[1] = (unsigned char)(pos >> 7) & 0x7F;
tx[2] = (unsigned char) pos & 0x7F;
UART_servo_PutArray(tx, 3);
CyDelayUs(500);
CyDelay(1);
}

/* [] END OF FILE */
8 changes: 2 additions & 6 deletions Design01.cydsn/Servo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,21 +22,17 @@ typedef struct{
uint8 id;
uint8 speed;
uint8 stretch;
uint8 state;
int angle;
} Servo_Data;
void Servo_Dataset(Servo_Data*, uint8 id, uint8 speed, uint8 stretch, int angle);
void EEPROM_rx(Servo_Data*);
void read_ID(void);
cystatus init_ID(uint8 id);
cystatus init_stretch(Servo_Data*);
cystatus init_speed(Servo_Data*);
cystatus speed_set(Servo_Data*);
void angle_set(Servo_Data*, int16 angle);
void angle_get(uint8 id);
void angle_keep(Servo_Data*);
/*
void angle_set(Servo_Data*, int16 angle);
void angle_get(Servo_Data*);
*/

#endif /* CYAPICALLBACKS_H */
/* [] END OF FILE */
152 changes: 129 additions & 23 deletions Design01.cydsn/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,60 +16,166 @@

Servo_Data servo[3];

void free(void);
void kamae(void);
void karimen(void);
cystatus speed(void);
void free(uint16 *pwm_data);
void kamae(uint16 *pwm_data);
void men(uint16 *pwm_data);
void hukamen(uint16 *pwm_data);
void dou(uint16 *pwm_data);
void keep(uint16 *pwm_data);

int main()
{
bit free_flag = 1;
uint16 pwm_data = 157;
char buf[32];
cystatus speed_sts;
PS2Controller psData;
UART_servo_Start();
UART_PC_Start();
PWM_Start(); /* 73 ~ 233 中心153 */
PS2_Start();
CyGlobalIntEnable; /* Enable global interrupts. */
/* */
CyDelay(1000);
sprintf(buf, "Servo Start\n");
UART_PC_PutString(buf);
Servo_Dataset(&servo[0], 0, 100, 10, -5);
Servo_Dataset(&servo[1], 1, 100, 10, -70);
Servo_Dataset(&servo[2], 2, 100, 10, 130);
Servo_Dataset(&servo[0], 0, 100, 10, -27); /* -116 ~ 55 */
Servo_Dataset(&servo[1], 1, 100, 10, -70); /* 117 ~ -100 */
Servo_Dataset(&servo[2], 2, 100, 10, 110); /* -135 ~ 135 */
CyDelay(1000);
/*
servo[0].speed = 127;
servo[1].speed = 127;
servo[2].speed = 127;
speed_sts = speed();
if(speed_sts == CYRET_SUCCESS){
sprintf(buf, "speed: SUCCESS\n");
UART_PC_PutString(buf);
} else if(speed_sts == CYRET_EMPTY){
sprintf(buf, "speed: EMPTY\n");
UART_PC_PutString(buf);
}
*/
for(;;)
{
psData = PS2_Controller_get();
sprintf(buf, "%d\n", (int)psData.CIRCLE);
UART_PC_PutString(buf);
/* 脱力モード */
while(free_flag == 1){
free(&pwm_data);
psData = PS2_Controller_get();
if(psData.START){
free_flag = 0;
}
}
if(psData.SELECT){
if(psData.START){
free_flag = 1;
}
}

/* pwm_servo */
if(psData.L1){
pwm_data = 70;
PWM_WriteCompare(pwm_data);
} else if(psData.R1){
pwm_data = 230;
PWM_WriteCompare(pwm_data);
} else{
pwm_data = 157;
PWM_WriteCompare(pwm_data);
}

/* servo0 */
if(psData.LEFT){
angle_set(&servo[0], -80);
} else if(psData.RIGHT){
angle_set(&servo[0], 30);
} else {
angle_set(&servo[0], -27);
}

/* servo1 */
if(psData.UP){
angle_set(&servo[1], 20);
} else if(psData.DOWN){
//angle_set(&servo[1], -70);
} else{
angle_set(&servo[1], -70);
}

/* 技 */
if(psData.CIRCLE){
kamae();
men(&pwm_data);
} else if(psData.CROSS){
hukamen(&pwm_data);
} else if(psData.SQUARE){
dou(&pwm_data);
} else {
pwm_data = 157;
}
if(psData.START){
karimen();
/* servo2 */
if(psData.R2){
angle_set(&servo[2], 30);
} else if((psData.CIRCLE != 1) && (psData.CROSS != 1) && (psData.SQUARE != 1)){
angle_set(&servo[2], 110);
}
angle_keep(&servo[0]);
angle_keep(&servo[1]);
angle_keep(&servo[2]);
keep(&pwm_data);
}
}

void free(void) {
cystatus speed(void){
cystatus speed;
speed = speed_set(&servo[0]);
if(speed == CYRET_EMPTY){
return CYRET_EMPTY;
}
speed = speed_set(&servo[1]);
if(speed == CYRET_EMPTY){
return CYRET_EMPTY;
}
speed = speed_set(&servo[2]);
if(speed == CYRET_EMPTY){
return CYRET_EMPTY;
}
return CYRET_SUCCESS;
}

void free(uint16 *pwm_data) {
angle_get(servo[0].id);
angle_get(servo[1].id);
angle_get(servo[2].id);
PWM_WriteCompare(*pwm_data);
}

void kamae(void) {
angle_set(&servo[0], -5);
void kamae(uint16 *pwm_data) {
angle_set(&servo[1], -70);
angle_set(&servo[2], 130);
PWM_WriteCompare(133);
angle_set(&servo[2], 110);
PWM_WriteCompare(*pwm_data);
}

void men(uint16 *pwm_data) {
angle_set(&servo[1], 45);
angle_set(&servo[2], -20);
PWM_WriteCompare(*pwm_data);
}

void hukamen(uint16 *pwm_data){
angle_set(&servo[1], 75);
angle_set(&servo[2], -20);
PWM_WriteCompare(*pwm_data);
}

void karimen(void){
angle_set(&servo[2], 0);
angle_set(&servo[0], -5);
void dou(uint16 *pwm_data){
angle_set(&servo[1], 45);
PWM_WriteCompare(150);
angle_set(&servo[2], -5);
PWM_WriteCompare(*pwm_data);
}

void keep(uint16 *pwm_data){
angle_keep(&servo[0]);
angle_keep(&servo[1]);
angle_keep(&servo[2]);
PWM_WriteCompare(*pwm_data);
}
/* [] END OF FILE */

0 comments on commit fc495a1

Please sign in to comment.