From 9af54029a8d17b94bfed3b055aecc75af408b85d Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Fri, 22 Nov 2019 17:51:58 +0100
Subject: [PATCH 01/20] more stuff on the arm
---
arm/arm/arm.c | 83 ++++++++++++++++++++++++-----------------
arm/arm/arm.h | 3 ++
arm/arm/main.c | 44 ++++++++++++++++++----
arm/arm/serial.c | 2 +-
arm/arm/styr_komm.c | 3 +-
arm/arm/styrmodul.cproj | 2 +-
6 files changed, 93 insertions(+), 44 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 3598d063..e3ec121b 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -12,10 +12,8 @@
#include <string.h>
#include "arm.h"
-
#define F_CPU 16000000L
-
//Array of instruction names. The index of the instruction string is the address of the instruction.
const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
"Model Number",
@@ -23,7 +21,7 @@ const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
"Firmware Version",
"ID",
"Baud Rate",
- "Return Delay Time",
+ "Return Delay Time",//330
"CW Angle Limit",
"",
"CCW Angle Limit",
@@ -69,6 +67,8 @@ const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
"Punch"
};
+
+
/**
* Sets the position limits for all servos
*/
@@ -86,10 +86,10 @@ servo_init(){
set_ccw_limit(4,0x03FF,IMODE_WRITE);
_delay_ms(100);
set_cw_limit(5,0x0000,IMODE_WRITE);//TODO check limits
- set_ccw_limit(5,0x03FF,IMODE_WRITE);
+ set_ccw_limit(5,0x0330,IMODE_WRITE);
_delay_ms(100);
set_cw_limit(6,0x00FF,IMODE_WRITE);
- set_ccw_limit(6,0x0333,IMODE_WRITE);
+ set_ccw_limit(6,0x0330,IMODE_WRITE);
_delay_ms(100);
set_cw_limit(7,0x0000,IMODE_WRITE);
set_ccw_limit(7,0x03FF,IMODE_WRITE);
@@ -133,22 +133,32 @@ void set_moving_speed(uint8_t id, uint16_t speed, uint8_t imode){
write_two_bytes(id, "Moving Speed", speed, 2, imode);
}
+void set_max_torque(uint8_t id, uint16_t torque, uint8_t imode){
+ write_two_bytes(id, "Max Torque", torque, 2, imode);
+}
+
+void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode){
+ write_two_bytes(id, "Baud Rate", baud_rate, 1, imode);
+}
+
void arm_rotate_90_left(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x00D5,imode);
}
void lower_arm(){
- move_to_angle(5, 0x00FF, 0x00EF, IMODE_WRITE); //4 straight up
+
+
+
+
+}
+
+void raise_arm(){
+ move_to_angle(2,0x00FF,0x031F,IMODE_WRITE);
_delay_ms(5000);
- move_to_angle(2, 0x00FF, 0x00EF, IMODE_REG_WRITE); //2 straight up
- //_delay_ms(5000);
- move_to_angle(6, 0x00FF, 0x027F, IMODE_REG_WRITE);
- move_to_angle(5, 0x00FF, 0x01FF, IMODE_REG_WRITE);
+ move_to_angle(5,0x00FF,0x0330,IMODE_REG_WRITE);
+ move_to_angle(6,0x00FF,0x01FF,IMODE_REG_WRITE);
send_action();
- _delay_ms(5000);
-
}
-/**/
/**
* Moves the servo with the given id to a given goal position with the given speed
@@ -165,31 +175,36 @@ void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t i
uint8_t higher_byte_speed = speed >> 8;
uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos, higher_byte_pos, lower_byte_speed, higher_byte_speed};
-
- if (id == 2 || id == 3 || id == 4 || id == 5){//Servo-pairs (2 and 3) and (4 and 5) should always move together with "opposite" goal_position
- uint16_t goal_position_2 = 0x03FF - goal_position_2;
+ send_instruction(id, imode, params, 5);
+
+ if (id == 2){//Servo 2 and 3 should always move together
+ uint16_t goal_position_3 = 0x03FF - goal_position;
+ uint8_t lower_byte_pos_3 = goal_position_3 & 0xFF;
+ uint8_t higher_byte_pos_3 = goal_position_3 >> 8;
+ uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_3, higher_byte_pos_3, lower_byte_speed, higher_byte_speed};
+ send_instruction(3, imode, params, 5);
+ }
+ else if(id == 3){
+ uint16_t goal_position_2 = 0x03FF - goal_position;
uint8_t lower_byte_pos_2 = goal_position_2 & 0xFF;
uint8_t higher_byte_pos_2 = goal_position_2 >> 8;
uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_2, higher_byte_pos_2, lower_byte_speed, higher_byte_speed};
- switch (id) {
- case 2:
- send_instruction(3, imode, params, 5);
- break;
- case 3:
- send_instruction(2, imode, params, 5);
- break;
- case 4:
- send_instruction(5, imode, params, 5);
- break;
- case 5:
- send_instruction(4, imode, params, 5);
- break;
- default:
- break;
- }
+ send_instruction(4, imode, params, 5);
+ }
+ if (id == 4){//Servo 4 and 5 should always move together
+ uint16_t goal_position_5 = 0x03FF - goal_position;
+ uint8_t lower_byte_pos_5 = goal_position_5 & 0xFF;
+ uint8_t higher_byte_pos_5 = goal_position_5 >> 8;
+ uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_5, higher_byte_pos_5, lower_byte_speed, higher_byte_speed};
+ send_instruction(5, imode, params, 5);
+ }
+ else if(id == 5){
+ uint16_t goal_position_4 = 0x03FF - goal_position;
+ uint8_t lower_byte_pos_4 = goal_position_4 & 0xFF;
+ uint8_t higher_byte_pos_4 = goal_position_4 >> 8;
+ uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_4, higher_byte_pos_4, lower_byte_speed, higher_byte_speed};
+ send_instruction(4, imode, params, 5);
}
-
- send_instruction(id, imode, params, 5);
}
/**
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index b23d5fb1..788e29d7 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -60,5 +60,8 @@ void write_two_bytes(uint8_t id, char *name, uint16_t arg, uint8_t param_len, ui
void send_instruction(uint8_t id, uint8_t imode, uint8_t *params, uint8_t param_len);
void arm_rotate_90_left(uint16_t speed,uint8_t imode);
void set_return_delay(uint8_t id, uint16_t delay, uint8_t imode);
+void set_max_torque(uint8_t id, uint16_t torque, uint8_t imode);
+void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode);
+void raise_arm();
#endif ARM_H_
\ No newline at end of file
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 35cbfbdd..ecacd719 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -48,36 +48,66 @@ int main(void){
}
// init
- servo_init();
+ //servo_init();
pwm_init();
+ set_return_delay(1,254,IMODE_WRITE);
+ set_baud_rate(0xFE,103,IMODE_WRITE);
+
+ //set_max_torque(4, 0x300,IMODE_WRITE);
+ //set_max_torque(5, 0x300,IMODE_WRITE);
/************************************************************************/
/* TEST BELOW */
/************************************************************************/
- //set_return_delay(1,250,IMODE_WRITE);
+ move_to_angle(1,0x00FF,0x01FF,IMODE_WRITE);
+ _delay_ms(10000);
arm_rotate_90_left(0x00FF,IMODE_WRITE);
+ _delay_ms(3000);
+ //lower_arm();
+ move_to_angle(6, 0x00AF, 0x02FF, IMODE_REG_WRITE);
+ move_to_angle(5, 0x00AF, 0x01FF, IMODE_REG_WRITE);
+ send_action();
+ _delay_ms(5000);
+ move_to_angle(2, 0x00AF, 0x013F, IMODE_WRITE);
+ _delay_ms(4000);
+ raise_arm();
+ _delay_ms(5000);
+ move_to_angle(6, 0x00AF, 0x02FF, IMODE_REG_WRITE);
+ move_to_angle(5, 0x00AF, 0x01FF, IMODE_REG_WRITE);
+ send_action();
+ _delay_ms(5000);
+ move_to_angle(2, 0x00AF, 0x013F, IMODE_WRITE);
+ _delay_ms(4000);
+ raise_arm();
_delay_ms(5000);
- lower_arm();
+ move_to_angle(1,0x00FF,0x01FF,IMODE_WRITE);
/************************************************************************/
/* TEST ABOVE */
/************************************************************************/
- /*UCSR1B &= ~(_BV(RXEN1));
+ UCSR1B &= ~(_BV(RXEN1));
UCSR1B |= _BV(TXEN1); //~(_BV(U2X0));
ping(1);
DDRD = 0;
UCSR1B &= ~(_BV(TXEN1));
UCSR1B |= _BV(RXEN1);
- /*volatile uint8_t b1, b2, b3,b4,b5;
+ volatile uint8_t b1, b2, b3,b4,b5,b6,b7,b8,b9,b10;
while(1){
b1 = uart_1_getchar();
b2 = uart_1_getchar();
b3 = uart_1_getchar();
b4 = uart_1_getchar();
b5 = uart_1_getchar();
+ b6 = uart_1_getchar();
+ b7 = uart_1_getchar();
+ b8 = uart_1_getchar();
+ b9 = uart_1_getchar();
+ b10 = uart_1_getchar();
+ }
+
//translate_instruction(b1,b2,b3);
- }
- */
+
+
return 0;
}
diff --git a/arm/arm/serial.c b/arm/arm/serial.c
index 440c7818..52ef7ee5 100644
--- a/arm/arm/serial.c
+++ b/arm/arm/serial.c
@@ -4,7 +4,7 @@
*/
#define F_CPU 16000000L
-#define BAUD 19200 // for serial 1M
+#define BAUD 19200 // for serial 1M
#include <avr/io.h>
#include <stdio.h>
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index e6f7ac08..88ca984c 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -36,7 +36,8 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 7://Set driving speed
set_speed(arg1);
break;
- case 8:
+ case 8://Increment servo
+
break;
case 9:
break;
diff --git a/arm/arm/styrmodul.cproj b/arm/arm/styrmodul.cproj
index 22033b1b..c85bf47c 100644
--- a/arm/arm/styrmodul.cproj
+++ b/arm/arm/styrmodul.cproj
@@ -47,7 +47,7 @@
<InterfaceName>JTAG</InterfaceName>
</ToolOptions>
<ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
- <ToolNumber>J41800087887</ToolNumber>
+ <ToolNumber>J41800055572</ToolNumber>
<ToolName>Atmel-ICE</ToolName>
</com_atmel_avrdbg_tool_atmelice>
<avrtoolinterface>JTAG</avrtoolinterface>
--
GitLab
From 09f78ca52016295a40f0a8c235b4452e2a1895eb Mon Sep 17 00:00:00 2001
From: Jonatan Jonsson <jonjo049@student.liu.se>
Date: Mon, 25 Nov 2019 15:45:33 +0100
Subject: [PATCH 02/20] added most of the instructions in the switch case
---
arm/arm/arm.c | 69 ++++++++++++++++++++++----------------------
arm/arm/arm.h | 7 ++++-
arm/arm/main.c | 61 ++++++++-------------------------------
arm/arm/steering.c | 1 +
arm/arm/steering.h | 1 +
arm/arm/styr_komm.c | 37 +++++++++++++++++-------
arm/styrmodul.atsuo | Bin 18432 -> 18944 bytes
7 files changed, 82 insertions(+), 94 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index e3ec121b..eccaafc2 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -4,15 +4,14 @@
* Created: 2019-11-15 14:28:20
* Author: niljo502
*/
+#define F_CPU 16000000L
#include <avr/io.h>
-
#include <util/delay.h>
#include <stdio.h>
#include <string.h>
#include "arm.h"
-#define F_CPU 16000000L
//Array of instruction names. The index of the instruction string is the address of the instruction.
const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
@@ -143,13 +142,6 @@ void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode){
void arm_rotate_90_left(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x00D5,imode);
-}
-
-void lower_arm(){
-
-
-
-
}
void raise_arm(){
@@ -157,17 +149,30 @@ void raise_arm(){
_delay_ms(5000);
move_to_angle(5,0x00FF,0x0330,IMODE_REG_WRITE);
move_to_angle(6,0x00FF,0x01FF,IMODE_REG_WRITE);
+}
+
+void lower_arm(){
+ move_to_angle(5, 0x00FF, 0x00EF, IMODE_REG_WRITE); //4 straight up
+ move_to_angle(6, 0x00FF, 0x027F, IMODE_REG_WRITE); //6 out of the way
+ send_action();
+ _delay_ms(7000); //This is not delaying?!
+ move_to_angle(2, 0x00FF, 0x00EF, IMODE_REG_WRITE); //2 straight up
+ move_to_angle(5, 0x00FF, 0x01FF, IMODE_REG_WRITE);
send_action();
}
/**
* Moves the servo with the given id to a given goal position with the given speed
* Sends an instruction that sets the speed and the goal position with only one package
+ * If the servo is paired, the instruction is also sent to its paired servo with its corresponding goal position,
+ * and their instructions are executed simultaneously.
+ * If servo is paired, the imode is always IMODE_REG_WRITE, and must be followed by a send_action() in order to execute.
*/
void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
if(id == ALL_SERVOS){//Please don't.
return;
}
+
uint8_t lower_byte_pos = goal_position& 0xFF;
uint8_t higher_byte_pos = goal_position >> 8;
@@ -175,35 +180,31 @@ void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t i
uint8_t higher_byte_speed = speed >> 8;
uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos, higher_byte_pos, lower_byte_speed, higher_byte_speed};
- send_instruction(id, imode, params, 5);
- if (id == 2){//Servo 2 and 3 should always move together
- uint16_t goal_position_3 = 0x03FF - goal_position;
- uint8_t lower_byte_pos_3 = goal_position_3 & 0xFF;
- uint8_t higher_byte_pos_3 = goal_position_3 >> 8;
- uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_3, higher_byte_pos_3, lower_byte_speed, higher_byte_speed};
- send_instruction(3, imode, params, 5);
- }
- else if(id == 3){
- uint16_t goal_position_2 = 0x03FF - goal_position;
+ if (id == 2 || id == 3 || id == 4 || id == 5){//Servo-pairs (2 and 3) and (4 and 5) should always move together with "opposite" goal_position
+ send_instruction(id, IMODE_REG_WRITE, params, 5);
+ uint16_t goal_position_2 = MAX_GOAL_POSITION - goal_position;
uint8_t lower_byte_pos_2 = goal_position_2 & 0xFF;
uint8_t higher_byte_pos_2 = goal_position_2 >> 8;
uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_2, higher_byte_pos_2, lower_byte_speed, higher_byte_speed};
- send_instruction(4, imode, params, 5);
- }
- if (id == 4){//Servo 4 and 5 should always move together
- uint16_t goal_position_5 = 0x03FF - goal_position;
- uint8_t lower_byte_pos_5 = goal_position_5 & 0xFF;
- uint8_t higher_byte_pos_5 = goal_position_5 >> 8;
- uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_5, higher_byte_pos_5, lower_byte_speed, higher_byte_speed};
- send_instruction(5, imode, params, 5);
- }
- else if(id == 5){
- uint16_t goal_position_4 = 0x03FF - goal_position;
- uint8_t lower_byte_pos_4 = goal_position_4 & 0xFF;
- uint8_t higher_byte_pos_4 = goal_position_4 >> 8;
- uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_4, higher_byte_pos_4, lower_byte_speed, higher_byte_speed};
- send_instruction(4, imode, params, 5);
+ switch (id) {
+ case 2:
+ send_instruction(3, IMODE_REG_WRITE, params, 5);
+ break;
+ case 3:
+ send_instruction(2, IMODE_REG_WRITE, params, 5);
+ break;
+ case 4:
+ send_instruction(5, IMODE_REG_WRITE, params, 5);
+ break;
+ case 5:
+ send_instruction(4, IMODE_REG_WRITE, params, 5);
+ break;
+ default:
+ break;
+ }
+ } else {
+ send_instruction(id, imode, params, 5);
}
}
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 788e29d7..2ead78f9 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -19,7 +19,12 @@
#define MAX_GOAL_POSITION 0x03FF
#define MIN_GOAL_POSITION 0x0000
-#define REST_POSITION_SERVO_1 0x01FF
+#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
+
+//TODO test these positions
+#define SERVO_90_RIGHT 205 //Position for 90 degrees to the right
+#define SERVO_90_LEFT 818 //Position for 90 degrees to the left
+
uint8_t get_addr(char name[]);
diff --git a/arm/arm/main.c b/arm/arm/main.c
index ecacd719..988cd853 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -48,65 +48,28 @@ int main(void){
}
// init
- //servo_init();
+ servo_init();
pwm_init();
- set_return_delay(1,254,IMODE_WRITE);
- set_baud_rate(0xFE,103,IMODE_WRITE);
-
- //set_max_torque(4, 0x300,IMODE_WRITE);
- //set_max_torque(5, 0x300,IMODE_WRITE);
/************************************************************************/
/* TEST BELOW */
/************************************************************************/
- move_to_angle(1,0x00FF,0x01FF,IMODE_WRITE);
- _delay_ms(10000);
- arm_rotate_90_left(0x00FF,IMODE_WRITE);
- _delay_ms(3000);
+ //set_return_delay(1,250,IMODE_WRITE);
+ //arm_rotate_90_left(0x00FF,IMODE_WRITE);
+ //_delay_ms(5000);
//lower_arm();
- move_to_angle(6, 0x00AF, 0x02FF, IMODE_REG_WRITE);
- move_to_angle(5, 0x00AF, 0x01FF, IMODE_REG_WRITE);
- send_action();
- _delay_ms(5000);
- move_to_angle(2, 0x00AF, 0x013F, IMODE_WRITE);
- _delay_ms(4000);
- raise_arm();
- _delay_ms(5000);
- move_to_angle(6, 0x00AF, 0x02FF, IMODE_REG_WRITE);
- move_to_angle(5, 0x00AF, 0x01FF, IMODE_REG_WRITE);
- send_action();
- _delay_ms(5000);
- move_to_angle(2, 0x00AF, 0x013F, IMODE_WRITE);
- _delay_ms(4000);
- raise_arm();
- _delay_ms(5000);
- move_to_angle(1,0x00FF,0x01FF,IMODE_WRITE);
+ //move_to_angle();
/************************************************************************/
/* TEST ABOVE */
/************************************************************************/
- UCSR1B &= ~(_BV(RXEN1));
- UCSR1B |= _BV(TXEN1); //~(_BV(U2X0));
- ping(1);
- DDRD = 0;
- UCSR1B &= ~(_BV(TXEN1));
- UCSR1B |= _BV(RXEN1);
-
- volatile uint8_t b1, b2, b3,b4,b5,b6,b7,b8,b9,b10;
- while(1){
- b1 = uart_1_getchar();
- b2 = uart_1_getchar();
- b3 = uart_1_getchar();
- b4 = uart_1_getchar();
- b5 = uart_1_getchar();
- b6 = uart_1_getchar();
- b7 = uart_1_getchar();
- b8 = uart_1_getchar();
- b9 = uart_1_getchar();
- b10 = uart_1_getchar();
- }
-
- //translate_instruction(b1,b2,b3);
+ uint8_t b1, b2, b3;
+ while (true) {
+ b1 = getchar();
+ b2 = getchar();
+ b3 = getchar();
+ translate_instruction(b1,b2,b3);
+ }
return 0;
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index 088ca49f..a0a1dd46 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -21,6 +21,7 @@ uint8_t left_speed = 0;
uint8_t default_speed = DEFAULT_SPEED;
+
enum Direction left_dir;//Current Direction of the left wheel pair
enum Direction right_dir;
enum Direction DEFAULT_DIR = FORWARD;
diff --git a/arm/arm/steering.h b/arm/arm/steering.h
index 99d180f2..1b1317dc 100644
--- a/arm/arm/steering.h
+++ b/arm/arm/steering.h
@@ -14,6 +14,7 @@ enum Direction{BACK, FORWARD};
#define DEFAULT_SPEED 100
#define DEFAULT_ROTATE_SPEED 100
+#define TURN_CONSTANT 0.5
#define ROTATE_90_DELAY 2000
#define DRIVE_ONE_BLOCK_DELAY 3400
#define TEST_SPEED 80
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 88ca984c..08bcfacb 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -11,7 +11,7 @@
void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
switch (instr) {
- case 255:
+ case 255: //Identify
putchar(10);
break;
case 0: //Forward
@@ -22,13 +22,21 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
set_direction(BACK);
set_speed(arg1);
break;
- case 2://Turn left
+ case 2://Turn left (Noting?)
+ set_right_speed(arg1);
+ set_left_speed(arg1*TURN_CONSTANT);
break;
- case 3: //Turn right
+ case 3: //Turn right (Noting?)
break;
- case 4: //Rotate left
+ case 4: //Rotate left
+ //Setup directions for rotation. Needs a speed to initiate.
+ set_speed(arg1);
+ rotate_left();
break;
case 5: //Rotate right
+ //Setup directions for rotation. Needs a speed to initiate.
+ set_speed(arg1);
+ rotate_right();
break;
case 6://STOP
set_speed(0);
@@ -36,16 +44,25 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 7://Set driving speed
set_speed(arg1);
break;
- case 8://Increment servo
-
+ case 8: //set angle
+ set_goal_position(arg1, arg2, IMODE_WRITE);
+ break;
+ case 9: //stop servo
+ break;
+ case 10: //get angle
+ break;
+ case 11: //get temperature
break;
- case 9:
+ case 12: //get voltage
break;
- case 10:
+ case 13: //set servo speed
+ set_moving_speed(arg1, arg2, IMODE_WRITE);
break;
- case 11:
+ case 50: //rotate 90 left
+ rotate_90_left();
break;
- case 12:
+ case 51: //rotate 90 right
+ rotate_right();
break;
case 100:
// Drive to next node
diff --git a/arm/styrmodul.atsuo b/arm/styrmodul.atsuo
index 7601e135512345bc9f723d4a6542360f78fd58b3..56034ccee448241927bbaf2a4640c388297356c3 100644
GIT binary patch
delta 800
zcma)4T}V@57(U<eJ7>pvR&%o@I&0IToyM?Di$WBSX&|=DrIBEYSV}j6&<KAZCkjOf
zx$|^WL>OLFSN)v(TI7}efKeAg7u^)ybu);fX78sBcG(LL-}^rA|I3$NX6c*k@ikeg
zlO!qsBzcM1Ji&G&%=da$R+puP)eo$MTxm@6b%m*<Vxuc+dpM?)gf5dEA#9RhgB!>x
zuX$b=YpPKp6oEF1DCqdD2KgCLCg@7Ad?vY1A17;q@IbJ4T$P!Dg!hehb>&gb*%SBH
z{`AiMxUqnwKjv7PCTj*oQO)gkDR-Y9Btk1DbM67Hx|m3nom8YMV#Mf9t}MVYY4##-
zCNS)6!;*0XJL-M7?LF<J%NI*rtrR*awP394u-M%>wyT-3ZcGP`>n7Pd2!#opTp+Eg
zVmYclQ`4i1gz}<*>KGY22}0ab>DJ36CSD{D#T{!;M8;_QLEH^ZC}aQncA_sFViGQf
z58zX~fo#}P6w3I97qzc&JsPCL+qLhItxMy(H^VMq))>M^uZbDMhfw`3yP=D_MGM*(
zsfI;1f}aiR9_Au0OeoCn-yWS9l5?r1#__3f_|0Lsy&dqGK0Grm()-b~|1wUMT6ScL
zO*+$&Ikt5rN<w)Z96K79@sh_?ix^vWel@Bx`-+az=g7tvvBqQSdGhECGfh35i!?D<
zuBlJ2C&NQ1Oys1Rm()zsg4Ghs48)X&<fD%{#GbZxtC~1MArh&h@3qPz!bx>5aa@U-
j6`hltZ?i-F_dm}EmD$0OF9C|eyw#3TfB3%<(yIOf8l4Ha
delta 1043
zcmY+@OH30%7zgm#vfY-tm6l*dEoy6NN=5C1L~LwqMG;FQ7&Mwt6vS6dC|IgeKxq}9
zph7<{CZM7SCzZG{L{21d@EOtI(Sz}3qNgTCW6=LLsCAm(&dm2sXJ;of@f%#cj~lzB
zkcK@WuD{-x<bzy3nfMjjzA<H;eds-x#ZV~5zo04Jnbh*srVNTI#O&}3h2HWON-1CQ
zA$j1mV9C3R#vC%<`TDw?_$e59IY>rTu%vcAQ-^vV?1y@2fCJD7O>hVfL;8;cs{BwG
zon~l(;}C&Xh{6dt38$b9V$cp9&<UsE40J&p1}n3_d>9H>QEI|6`uV{l>K7f7Ppwwa
zGj}mXHPxaGqqOkvGc~)^OEUw}1yx#3K*W?0l~M}~Jbkm(P=$6iz0u{;YW;4iFIq@D
zEa#-SzB?<+haK6uYLisH$i;C%iWw?JHD*}&-z<i>AnZZJz)^caK$KUZ&lr&BKu065
zPp%n{RyL!YZKaZLxn1EdL9~PUnTwUPt!zCj!PCjsu+8bV8!ab(TbKhae>&%<!Iek2
z!&umM3Yg5)Vk*{sjLzLH8=uiXT*~e|&-pclvyn%WCLet@zaxWrf<#egVJv-0SfnY`
z5NTT5*b-?z*16XcDr-IwX+6{$aygtEXmVAYJ4(jFO8R68Kq)QJ=hBhFn@L+fHfvqd
zx<!<ySqy(SJZkOcX6d^Xr*E;B$Y!B@TaYxm9W-w9(QCh(dUQ)DP;`<6voC3H<2t37
zy^B*F#3U$`$nK(Wu}Cv!qcl=npwM#55LUpSR|r0IhB@iST0ueal3Hsc%lh2&WVH79
z%hHOcloPJ;$g7fvXBJ90r$io!!uh0p4FvhM2}bR%08P6clyX&z7KD@;W2C6NSMVkf
zISc2Y8_vT8xCocvGF*Wk=!F5e2J%IiBd^C@G>G0UxD9t;2=2l?7>4^W0;AH9XFsmC
c`timJL2X1t{*3gcjc%S*Ycc{-w5*r^4W7e6A^-pY
--
GitLab
From 011dda926d71000102b7455a2c6a4d06011dfedd Mon Sep 17 00:00:00 2001
From: Jonatan Jonsson <jonjo049@student.liu.se>
Date: Mon, 25 Nov 2019 17:22:01 +0100
Subject: [PATCH 03/20] Added some docmentation and tried to rework uart
commuincation for the arm
---
arm/arm/arm.c | 6 ++++++
arm/arm/arm.h | 1 +
arm/arm/serial.c | 11 ++++++++++-
arm/arm/styr_komm.c | 27 +++++++++++++++++++++++++++
arm/styrmodul.atsuo | Bin 18944 -> 18944 bytes
5 files changed, 44 insertions(+), 1 deletion(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index eccaafc2..c062b9e1 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -71,6 +71,7 @@ const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
/**
* Sets the position limits for all servos
*/
+
servo_init(){
set_cw_limit(1,0x0000,IMODE_WRITE);
set_ccw_limit(1,0x03FF,IMODE_WRITE);
@@ -144,6 +145,11 @@ void arm_rotate_90_left(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x00D5,imode);
}
+void arm_rotate_90_right(uint16_t speed,uint8_t imode){
+ // TODO write function
+ //move_to_angle(1, speed, 0x00D5,imode);
+}
+
void raise_arm(){
move_to_angle(2,0x00FF,0x031F,IMODE_WRITE);
_delay_ms(5000);
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 2ead78f9..4aab18be 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -10,6 +10,7 @@
#define INSTRUCTION_SET_LENGTH 49
#define ALL_SERVOS 0xFE //ID for calling all servos
+#define CLAW_ID 8 // TODO check id of claw
#define IMODE_READ 2
#define IMODE_WRITE 3
diff --git a/arm/arm/serial.c b/arm/arm/serial.c
index 52ef7ee5..70ab0b44 100644
--- a/arm/arm/serial.c
+++ b/arm/arm/serial.c
@@ -24,8 +24,17 @@ char uart_0_getchar(FILE *stream){
}
void uart_1_putchar(char c){
- loop_until_bit_is_set(UCSR1A, UDRE1);
+ loop_until_bit_is_set(UCSR1A, UDRE1);//Wait until data register is empty and ready to receive new data
+ DDRD = 1; //Enable output, disable input
+ UCSR1B &= ~(_BV(RXEN1));
+ UCSR1B |= _BV(TXEN1);
+
UDR1 = c;
+ loop_until_bit_is_set(UCSR1A, TXC1);// Wait until transmission of data is complete
+
+ DDRD = 0;//Disable output, enable input
+ UCSR1B &= ~(_BV(TXEN1));
+ UCSR1B |= _BV(RXEN1);
}
char uart_1_getchar(){
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 08bcfacb..4e4c95f3 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -9,6 +9,14 @@
#include "steering.h"
#include "arm.h"
+/**
+ * Executes an instruction according to the given instruction number and the given arguments
+ *
+ * param instr: instruction number used to identify the instruction
+ * param arg1: first argument to instruction
+ * param arg2: second argument to instruction
+ *
+ */
void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
switch (instr) {
case 255: //Identify
@@ -58,6 +66,25 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 13: //set servo speed
set_moving_speed(arg1, arg2, IMODE_WRITE);
break;
+ case 15: //open claw
+ move_to_angle(CLAW_ID, arg1, 512, IMODE_WRITE); //TODO check if CLAW_ID and given goal_position is right
+ break;
+ case 16: //close claw
+ move_to_angle(CLAW_ID, arg1, 0, IMODE_WRITE);//TODO check if CLAW_ID and given goal_position is right
+ break;
+ case 17: // rotate arm left
+ break;
+ case 18: //rotate arm right
+ break;
+ case 19: //arm down
+ break;
+ case 20://arm up
+ break;
+ case 21: //arm forward
+ break;
+ case 22: //arm backward
+ break;
+
case 50: //rotate 90 left
rotate_90_left();
break;
diff --git a/arm/styrmodul.atsuo b/arm/styrmodul.atsuo
index 56034ccee448241927bbaf2a4640c388297356c3..f3471f7490a994cdc823ed8114541c2524254a5d 100644
GIT binary patch
delta 184
zcmZpe!q_l{aYHT(OG$~qg3X02znK_gCNH#7o;-n_NAN!q$Yx+*$N|a(Z{l!fntYhW
zaB~Tlj}oKLWDZfQ$qtGfdWj5047m(?3>geRL<`dOc>5WDO@2R3cAkg?BTy@nNnqw?
wLtP<OUPcz6lqytu@<kSv$t6bqAj4Usfr5pTe;Rr7r~p~KNJ=)B825+)0661JTmS$7
delta 165
zcmZpe!q_l{aYHT(%i;+P&6^8Zelty8VWl#80y~dj1_J{_CJ_Jo|NnnBkj|OB&?<Nn
zhcgo|BT(eO3Q!0{Prk^avbluIM@cl9p@hMRL60Gop#q3`8MuHd7$zqwYfpY?t1&s>
wU4L`9?i-fLhgl3Kml*l8GO~cAFS_WkrUMzhlYbg{^QZz@yg-aF*tkaw0ADyZ*8l(j
--
GitLab
From a75cac370acbbcdb3874a8c1fd18632f19392135 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Wed, 27 Nov 2019 11:49:31 +0100
Subject: [PATCH 04/20] Added a new instruction and worked more on the arm
---
arm/arm/arm.c | 32 +++++++++++++++++++-------------
arm/arm/arm.h | 6 ++++--
arm/arm/main.c | 27 +++++++++++++++++++++++----
arm/arm/serial.c | 8 +++++---
arm/arm/steering.c | 7 +++++++
arm/arm/steering.h | 1 +
arm/arm/styr_komm.c | 9 +++++++++
7 files changed, 68 insertions(+), 22 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index c062b9e1..64fc7ceb 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -12,6 +12,7 @@
#include <string.h>
#include "arm.h"
+#include "serial.h"
//Array of instruction names. The index of the instruction string is the address of the instruction.
const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
@@ -72,7 +73,7 @@ const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
* Sets the position limits for all servos
*/
-servo_init(){
+void servo_init(){
set_cw_limit(1,0x0000,IMODE_WRITE);
set_ccw_limit(1,0x03FF,IMODE_WRITE);
_delay_ms(100);
@@ -141,15 +142,6 @@ void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode){
write_two_bytes(id, "Baud Rate", baud_rate, 1, imode);
}
-void arm_rotate_90_left(uint16_t speed,uint8_t imode){
- move_to_angle(1, speed, 0x00D5,imode);
-}
-
-void arm_rotate_90_right(uint16_t speed,uint8_t imode){
- // TODO write function
- //move_to_angle(1, speed, 0x00D5,imode);
-}
-
void raise_arm(){
move_to_angle(2,0x00FF,0x031F,IMODE_WRITE);
_delay_ms(5000);
@@ -158,13 +150,15 @@ void raise_arm(){
}
void lower_arm(){
- move_to_angle(5, 0x00FF, 0x00EF, IMODE_REG_WRITE); //4 straight up
+/*
+ move_to_angle(5, 0x00FF, 0x01FF, IMODE_REG_WRITE); //4 straight up
move_to_angle(6, 0x00FF, 0x027F, IMODE_REG_WRITE); //6 out of the way
send_action();
_delay_ms(7000); //This is not delaying?!
move_to_angle(2, 0x00FF, 0x00EF, IMODE_REG_WRITE); //2 straight up
move_to_angle(5, 0x00FF, 0x01FF, IMODE_REG_WRITE);
send_action();
+*/
}
/**
@@ -176,6 +170,7 @@ void lower_arm(){
*/
void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
if(id == ALL_SERVOS){//Please don't.
+ int test = 12;
return;
}
@@ -214,6 +209,17 @@ void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t i
}
}
+
+void arm_rotate_90_left(uint16_t speed,uint8_t imode){
+ move_to_angle(1, speed, 0x00D5,imode);
+}
+
+void arm_rotate_90_right(uint16_t speed,uint8_t imode){
+ // TODO write function
+ move_to_angle(1, speed, 0x02FF,imode);
+}
+
+
/**
* Set counter clockwise limit for servo position
*/
@@ -285,8 +291,8 @@ void ping(uint8_t id) {
uart_1_putchar(id); //ID
uart_1_putchar(2); //LEN
uart_1_putchar(1); //INST
- uart_1_putchar(0xFB);
- //uart_1_putchar(0xFF - (id + 2 + 1)); //Checksum calculation
+ //uart_1_putchar(0xFB);
+ uart_1_putchar(0xFF - (id + 2 + 1)); //Checksum calculation
}
/**
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 4aab18be..768c58ad 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -64,10 +64,12 @@ void write_two_bytes(uint8_t id, char *name, uint16_t arg, uint8_t param_len, ui
/* Sends an instruction to any servo. */
void send_instruction(uint8_t id, uint8_t imode, uint8_t *params, uint8_t param_len);
-void arm_rotate_90_left(uint16_t speed,uint8_t imode);
void set_return_delay(uint8_t id, uint16_t delay, uint8_t imode);
void set_max_torque(uint8_t id, uint16_t torque, uint8_t imode);
void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode);
void raise_arm();
+void lower_arm();
+void arm_rotate_90_right(uint16_t speed,uint8_t imode);
+void arm_rotate_90_left(uint16_t speed,uint8_t imode);
-#endif ARM_H_
\ No newline at end of file
+#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 988cd853..45ffcbba 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -49,7 +49,6 @@ int main(void){
// init
servo_init();
- pwm_init();
/************************************************************************/
/* TEST BELOW */
@@ -59,18 +58,37 @@ int main(void){
//_delay_ms(5000);
//lower_arm();
//move_to_angle();
+ /*ping(7);
+ b1 = uart_1_getchar();
+ b2 = uart_1_getchar();
+ b3 = uart_1_getchar();
+ b4 = uart_1_getchar();
+ b5 = uart_1_getchar();
+ b6 = uart_1_getchar();
+ */
/************************************************************************/
/* TEST ABOVE */
/************************************************************************/
- uint8_t b1, b2, b3;
-
+ volatile uint8_t b1, b2, b3,b4,b5,b6,b7,b8;
+ move_to_angle(6, 0x00FF, 0x02FF,IMODE_REG_WRITE);
+ _delay_ms(5000);
+ send_action();
+/* move_to_angle(3, 0x00FF, 358, IMODE_WRITE);
+ send_action();
+ _delay_ms(6000);
+ move_to_angle(3, 0x00FF,205,IMODE_WRITE);
+ send_action();
+*/
+// lower_arm();
+ /*
while (true) {
b1 = getchar();
b2 = getchar();
b3 = getchar();
translate_instruction(b1,b2,b3);
}
-
+ */
+
return 0;
}
@@ -81,6 +99,7 @@ bool cold(){
// Initializes serial communication.
uart_init();
uart_arm_init();
+ pwm_init();
stdout = &uart_output;
stdin = &uart_input;
diff --git a/arm/arm/serial.c b/arm/arm/serial.c
index 70ab0b44..477ab851 100644
--- a/arm/arm/serial.c
+++ b/arm/arm/serial.c
@@ -24,17 +24,19 @@ char uart_0_getchar(FILE *stream){
}
void uart_1_putchar(char c){
- loop_until_bit_is_set(UCSR1A, UDRE1);//Wait until data register is empty and ready to receive new data
+ /*loop_until_bit_is_set(UCSR1A, UDRE1);//Wait until data register is empty and ready to receive new data
DDRD = 1; //Enable output, disable input
UCSR1B &= ~(_BV(RXEN1));
UCSR1B |= _BV(TXEN1);
-
+ */
+ loop_until_bit_is_set(UCSR1A, UDRE1);
UDR1 = c;
+/*
loop_until_bit_is_set(UCSR1A, TXC1);// Wait until transmission of data is complete
-
DDRD = 0;//Disable output, enable input
UCSR1B &= ~(_BV(TXEN1));
UCSR1B |= _BV(RXEN1);
+*/
}
char uart_1_getchar(){
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index a0a1dd46..34f5e973 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -184,4 +184,11 @@ void back_one_block(){
set_speed(DEFAULT_SPEED);
_delay_ms(DRIVE_ONE_BLOCK_DELAY);
set_direction(FORWARD);
+}
+
+void small_forward(){
+ set_direction(FORWARD);
+ set_speed(DEFAULT_SPEED);
+ _delay_ms(1000);
+ set_speed(0);
}
\ No newline at end of file
diff --git a/arm/arm/steering.h b/arm/arm/steering.h
index 1b1317dc..2f79d689 100644
--- a/arm/arm/steering.h
+++ b/arm/arm/steering.h
@@ -43,6 +43,7 @@ void rotate_90_right();
void drive_one_block();
void back_one_block();
void set_default_speed(uint8_t duty_threshold);
+void small_forward();
#endif
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 4e4c95f3..e302ff32 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -35,6 +35,8 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
set_left_speed(arg1*TURN_CONSTANT);
break;
case 3: //Turn right (Noting?)
+ set_right_speed(arg1*TURN_CONSTANT);
+ set_left_speed(arg1);
break;
case 4: //Rotate left
//Setup directions for rotation. Needs a speed to initiate.
@@ -73,12 +75,16 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
move_to_angle(CLAW_ID, arg1, 0, IMODE_WRITE);//TODO check if CLAW_ID and given goal_position is right
break;
case 17: // rotate arm left
+ arm_rotate_90_left(arg1,IMODE_WRITE);
break;
case 18: //rotate arm right
+ arm_rotate_90_right(arg1,IMODE_WRITE);
break;
case 19: //arm down
+ lower_arm();
break;
case 20://arm up
+ //raise_arm();
break;
case 21: //arm forward
break;
@@ -91,6 +97,9 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 51: //rotate 90 right
rotate_right();
break;
+ case 52: //Small forward
+ small_forward();
+ break;
case 100:
// Drive to next node
break;
--
GitLab
From 148e2b77f726dd519ba3ceed743a36e2c6b05cf5 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Thu, 28 Nov 2019 12:11:35 +0100
Subject: [PATCH 05/20] Implemented working lower arm instruction
---
arm/arm/arm.c | 27 ++++++++++++++++++++-------
1 file changed, 20 insertions(+), 7 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 64fc7ceb..44600342 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -150,15 +150,27 @@ void raise_arm(){
}
void lower_arm(){
-/*
- move_to_angle(5, 0x00FF, 0x01FF, IMODE_REG_WRITE); //4 straight up
- move_to_angle(6, 0x00FF, 0x027F, IMODE_REG_WRITE); //6 out of the way
+ move_to_angle(1, 0x00FF, 818,IMODE_WRITE);
+ move_to_angle(6, 0x00FF, 0x03FF,IMODE_WRITE);
+ _delay_ms(2000);
+
+ move_to_angle(4, 0x00FF, 300, IMODE_REG_WRITE);
+ send_action();
+ _delay_ms(2000);
+
+ move_to_angle(3, 0x00FF, 550, IMODE_REG_WRITE);
+ send_action();
+ _delay_ms(2000);
+
+ move_to_angle(4, 0x00FF, 0x0340, IMODE_REG_WRITE);
+ move_to_angle(3, 0x00A0, 800, IMODE_REG_WRITE);
+ move_to_angle(8,0x00FF,0x01FF,IMODE_REG_WRITE);
send_action();
- _delay_ms(7000); //This is not delaying?!
- move_to_angle(2, 0x00FF, 0x00EF, IMODE_REG_WRITE); //2 straight up
- move_to_angle(5, 0x00FF, 0x01FF, IMODE_REG_WRITE);
+ _delay_ms(1000);
+
+ move_to_angle(6, 0x00FF, 0x01FF,IMODE_REG_WRITE);
+ move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);
send_action();
-*/
}
/**
@@ -184,6 +196,7 @@ void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t i
if (id == 2 || id == 3 || id == 4 || id == 5){//Servo-pairs (2 and 3) and (4 and 5) should always move together with "opposite" goal_position
send_instruction(id, IMODE_REG_WRITE, params, 5);
+ _delay_ms(200);
uint16_t goal_position_2 = MAX_GOAL_POSITION - goal_position;
uint8_t lower_byte_pos_2 = goal_position_2 & 0xFF;
uint8_t higher_byte_pos_2 = goal_position_2 >> 8;
--
GitLab
From 89c0014af6a569a8d9734f121dde64b5fc5cba68 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Thu, 28 Nov 2019 12:49:29 +0100
Subject: [PATCH 06/20] Implemented raise arm instruction
---
arm/arm/arm.c | 21 +++++++++++++++++----
arm/arm/arm.h | 1 +
arm/arm/styr_komm.c | 2 +-
3 files changed, 19 insertions(+), 5 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 44600342..547a8a10 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -142,14 +142,27 @@ void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode){
write_two_bytes(id, "Baud Rate", baud_rate, 1, imode);
}
+//Enable or disable torque for a servo with the given id
+void torque_enable(uint8_t id, uint16_t enable, uint8_t imode){
+ write_two_bytes(id,"Torque Enable", enable, 1, imode);
+}
+
void raise_arm(){
- move_to_angle(2,0x00FF,0x031F,IMODE_WRITE);
- _delay_ms(5000);
- move_to_angle(5,0x00FF,0x0330,IMODE_REG_WRITE);
- move_to_angle(6,0x00FF,0x01FF,IMODE_REG_WRITE);
+ move_to_angle(2,0x00AF,0x031F,IMODE_REG_WRITE);
+ move_to_angle(5,0x00AF,0x0300,IMODE_REG_WRITE);
+ move_to_angle(6,0x00AF,0x01AF,IMODE_REG_WRITE);
+ _delay_ms(2000);
+ send_action();
+ _delay_ms(2000);
+ torque_enable(2,0,IMODE_WRITE);//Disable servos 2 and 3 since they don't need to work
+ torque_enable(3,0,IMODE_WRITE);
+
}
void lower_arm(){
+ torque_enable(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
+ torque_enable(3,1,IMODE_WRITE);//we have to make sure that they are enabled here
+ _delay_ms(200);
move_to_angle(1, 0x00FF, 818,IMODE_WRITE);
move_to_angle(6, 0x00FF, 0x03FF,IMODE_WRITE);
_delay_ms(2000);
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 768c58ad..1c3c9781 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -71,5 +71,6 @@ void raise_arm();
void lower_arm();
void arm_rotate_90_right(uint16_t speed,uint8_t imode);
void arm_rotate_90_left(uint16_t speed,uint8_t imode);
+void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index e302ff32..557030af 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -84,7 +84,7 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
lower_arm();
break;
case 20://arm up
- //raise_arm();
+ raise_arm();
break;
case 21: //arm forward
break;
--
GitLab
From ff70a8baf6eeedc086f816573bab3d2d80ea679e Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Thu, 28 Nov 2019 16:16:06 +0100
Subject: [PATCH 07/20] wrote functionality for moving the arm slightly
forward/backward when the arm is lowered. Doesnt seem to really work because
the servos are weak?
---
arm/arm/arm.c | 72 +++++++++++++++++++++++++++++++++++++++++++++++----
arm/arm/arm.h | 8 ++++++
2 files changed, 75 insertions(+), 5 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 547a8a10..fb63da0a 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -14,6 +14,10 @@
#include "arm.h"
#include "serial.h"
+uint16_t servo_6_pos;
+uint16_t servo_4_pos;
+uint8_t arm_forward_callable = 0; //1 if instruction arm_forward should be callable 0 if not
+
//Array of instruction names. The index of the instruction string is the address of the instruction.
const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
"Model Number",
@@ -156,6 +160,7 @@ void raise_arm(){
_delay_ms(2000);
torque_enable(2,0,IMODE_WRITE);//Disable servos 2 and 3 since they don't need to work
torque_enable(3,0,IMODE_WRITE);
+ arm_forward_callable = 0;
}
@@ -163,7 +168,7 @@ void lower_arm(){
torque_enable(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
torque_enable(3,1,IMODE_WRITE);//we have to make sure that they are enabled here
_delay_ms(200);
- move_to_angle(1, 0x00FF, 818,IMODE_WRITE);
+
move_to_angle(6, 0x00FF, 0x03FF,IMODE_WRITE);
_delay_ms(2000);
@@ -175,17 +180,75 @@ void lower_arm(){
send_action();
_delay_ms(2000);
- move_to_angle(4, 0x00FF, 0x0340, IMODE_REG_WRITE);
+ move_to_angle(4, 0x00FF, 700, IMODE_REG_WRITE);
move_to_angle(3, 0x00A0, 800, IMODE_REG_WRITE);
move_to_angle(8,0x00FF,0x01FF,IMODE_REG_WRITE);
send_action();
_delay_ms(1000);
- move_to_angle(6, 0x00FF, 0x01FF,IMODE_REG_WRITE);
+ move_to_angle(6, 0x00FF, 640,IMODE_REG_WRITE);
move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);
send_action();
+ torque_enable(2,0,IMODE_WRITE);
+ torque_enable(3,0,IMODE_WRITE);
+ servo_4_pos = 700;
+ servo_6_pos = 640;
+ arm_forward_callable = 1;
+}
+
+/**
+ * Move arm slightly forward. Meant to be used after lower_arm() has been called!
+ * Do not call the function if the arm has not been lowered!!
+ */
+void arm_forward(){
+
+ if( (servo_6_pos-15) > servo_6_pos || (servo_6_pos-15) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
+ servo_6_pos = SERVO_6_LOWER_LIMIT;
+ }
+ else{
+ servo_6_pos = servo_6_pos - 20;
+ }
+
+ if( (servo_4_pos+40) < servo_4_pos || servo_4_pos+40 >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
+ servo_4_pos = SERVO_4_UPPER_LIMIT;
+ }
+ else{
+ servo_4_pos = servo_4_pos + 40;
+ }
+
+ move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
+ _delay_ms(200);
+ move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
+ send_action();
}
+/**
+ * Move arm slightly backward. Meant to be used after lower_arm() has been called!
+ * Do not call the function if the arm has not been lowered!!
+ */
+void arm_backward(){
+
+ if( (servo_6_pos+20) < servo_6_pos || (servo_6_pos+20) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
+ servo_6_pos = SERVO_6_UPPER_LIMIT;
+ }
+ else{
+ servo_6_pos = servo_6_pos + 20;
+ }
+
+ if( (servo_4_pos-40) > servo_4_pos || servo_4_pos-40 <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
+ servo_4_pos = SERVO_4_LOWER_LIMIT;
+ }
+ else{
+ servo_4_pos = servo_4_pos - 40;
+ }
+
+ move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
+ _delay_ms(200);
+ move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
+ send_action();
+}
+
+
/**
* Moves the servo with the given id to a given goal position with the given speed
* Sends an instruction that sets the speed and the goal position with only one package
@@ -241,8 +304,7 @@ void arm_rotate_90_left(uint16_t speed,uint8_t imode){
}
void arm_rotate_90_right(uint16_t speed,uint8_t imode){
- // TODO write function
- move_to_angle(1, speed, 0x02FF,imode);
+ move_to_angle(1, speed, 0x0332,imode);
}
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 1c3c9781..7ae85a19 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -20,6 +20,12 @@
#define MAX_GOAL_POSITION 0x03FF
#define MIN_GOAL_POSITION 0x0000
+#define SERVO_6_UPPER_LIMIT 0x0330
+#define SERVO_6_LOWER_LIMIT 0x00FF
+
+#define SERVO_4_UPPER_LIMIT 0x03FF
+#define SERVO_4_LOWER_LIMIT 0x0000
+
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
//TODO test these positions
@@ -72,5 +78,7 @@ void lower_arm();
void arm_rotate_90_right(uint16_t speed,uint8_t imode);
void arm_rotate_90_left(uint16_t speed,uint8_t imode);
void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
+void arm_forward();
+void arm_backward();
#endif // ARM_H_
\ No newline at end of file
--
GitLab
From 281c739784405726493bb37a63af94ad38b97294 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Fri, 29 Nov 2019 11:15:50 +0100
Subject: [PATCH 08/20] Fine tuned arm functions
---
arm/arm/arm.c | 41 +++++++++++++++++++++++------------------
1 file changed, 23 insertions(+), 18 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index fb63da0a..0b9361a3 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -16,6 +16,8 @@
uint16_t servo_6_pos;
uint16_t servo_4_pos;
+uint16_t servo_3_pos;
+
uint8_t arm_forward_callable = 0; //1 if instruction arm_forward should be callable 0 if not
//Array of instruction names. The index of the instruction string is the address of the instruction.
@@ -154,7 +156,7 @@ void torque_enable(uint8_t id, uint16_t enable, uint8_t imode){
void raise_arm(){
move_to_angle(2,0x00AF,0x031F,IMODE_REG_WRITE);
move_to_angle(5,0x00AF,0x0300,IMODE_REG_WRITE);
- move_to_angle(6,0x00AF,0x01AF,IMODE_REG_WRITE);
+ move_to_angle(6,0x00AF,0x03FF,IMODE_REG_WRITE);
_delay_ms(2000);
send_action();
_delay_ms(2000);
@@ -180,19 +182,22 @@ void lower_arm(){
send_action();
_delay_ms(2000);
- move_to_angle(4, 0x00FF, 700, IMODE_REG_WRITE);
- move_to_angle(3, 0x00A0, 800, IMODE_REG_WRITE);
+ move_to_angle(4, 0x00FF, 400, IMODE_REG_WRITE);
+ move_to_angle(3, 0x00A0, 700, IMODE_REG_WRITE);
move_to_angle(8,0x00FF,0x01FF,IMODE_REG_WRITE);
send_action();
_delay_ms(1000);
- move_to_angle(6, 0x00FF, 640,IMODE_REG_WRITE);
- move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);
+ move_to_angle(6, 0x00FF, 818,IMODE_REG_WRITE);
+ move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);//Open claw
send_action();
- torque_enable(2,0,IMODE_WRITE);
- torque_enable(3,0,IMODE_WRITE);
- servo_4_pos = 700;
- servo_6_pos = 640;
+ //torque_enable(2,0,IMODE_WRITE);//Relax servo 2 and 3 since they don't need to work in this position
+ //torque_enable(3,0,IMODE_WRITE);
+
+ servo_3_pos = 700;
+ servo_4_pos = 400;
+ servo_6_pos = 818;
+
arm_forward_callable = 1;
}
@@ -202,23 +207,23 @@ void lower_arm(){
*/
void arm_forward(){
- if( (servo_6_pos-15) > servo_6_pos || (servo_6_pos-15) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
+ if( (servo_6_pos-20) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
servo_6_pos = SERVO_6_LOWER_LIMIT;
}
else{
servo_6_pos = servo_6_pos - 20;
}
- if( (servo_4_pos+40) < servo_4_pos || servo_4_pos+40 >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
+ if( (servo_4_pos+20) >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
servo_4_pos = SERVO_4_UPPER_LIMIT;
}
else{
- servo_4_pos = servo_4_pos + 40;
+ servo_4_pos = servo_4_pos + 20;
}
move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
- _delay_ms(200);
move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
+ _delay_ms(100);
send_action();
}
@@ -228,23 +233,23 @@ void arm_forward(){
*/
void arm_backward(){
- if( (servo_6_pos+20) < servo_6_pos || (servo_6_pos+20) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
+ if( (servo_6_pos+20) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
servo_6_pos = SERVO_6_UPPER_LIMIT;
}
else{
servo_6_pos = servo_6_pos + 20;
}
- if( (servo_4_pos-40) > servo_4_pos || servo_4_pos-40 <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
+ if( servo_4_pos-20 <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
servo_4_pos = SERVO_4_LOWER_LIMIT;
}
else{
- servo_4_pos = servo_4_pos - 40;
+ servo_4_pos = servo_4_pos - 20;
}
-
+
move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
- _delay_ms(200);
move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
+ _delay_ms(100);
send_action();
}
--
GitLab
From 82af726e31add149733b6a905e231cd2e4d9b620 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Fri, 29 Nov 2019 11:53:19 +0100
Subject: [PATCH 09/20] Cleaned up the code
---
arm/arm/arm.c | 32 ++++++++++++++++----------------
arm/arm/arm.h | 2 ++
arm/arm/main.c | 34 ++++++++--------------------------
arm/arm/serial.c | 11 -----------
arm/arm/styr_komm.c | 13 ++++++++-----
arm/arm/styrmodul.cproj | 2 +-
arm/styrmodul.atsuo | Bin 18944 -> 19456 bytes
7 files changed, 35 insertions(+), 59 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 0b9361a3..2e1e4841 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -78,7 +78,6 @@ const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
/**
* Sets the position limits for all servos
*/
-
void servo_init(){
set_cw_limit(1,0x0000,IMODE_WRITE);
set_ccw_limit(1,0x03FF,IMODE_WRITE);
@@ -115,7 +114,7 @@ uint8_t get_addr(char name[]){
return i;
}
}
- return 254;
+ return 254;//Name not found
}
void set_led(uint8_t id, uint16_t led_value, uint8_t imode){
@@ -168,7 +167,7 @@ void raise_arm(){
void lower_arm(){
torque_enable(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
- torque_enable(3,1,IMODE_WRITE);//we have to make sure that they are enabled here
+ torque_enable(3,1,IMODE_WRITE);//we make sure that they are enabled here
_delay_ms(200);
move_to_angle(6, 0x00FF, 0x03FF,IMODE_WRITE);
@@ -191,8 +190,6 @@ void lower_arm(){
move_to_angle(6, 0x00FF, 818,IMODE_REG_WRITE);
move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);//Open claw
send_action();
- //torque_enable(2,0,IMODE_WRITE);//Relax servo 2 and 3 since they don't need to work in this position
- //torque_enable(3,0,IMODE_WRITE);
servo_3_pos = 700;
servo_4_pos = 400;
@@ -206,19 +203,22 @@ void lower_arm(){
* Do not call the function if the arm has not been lowered!!
*/
void arm_forward(){
+ if(arm_forward_callable != 1){
+ return;
+ }
- if( (servo_6_pos-20) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
+ if( (servo_6_pos-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
servo_6_pos = SERVO_6_LOWER_LIMIT;
}
else{
- servo_6_pos = servo_6_pos - 20;
+ servo_6_pos = servo_6_pos - ARM_FORWARD_DELTA;
}
- if( (servo_4_pos+20) >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
+ if( (servo_4_pos+ARM_FORWARD_DELTA) >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
servo_4_pos = SERVO_4_UPPER_LIMIT;
}
else{
- servo_4_pos = servo_4_pos + 20;
+ servo_4_pos = servo_4_pos + ARM_FORWARD_DELTA;
}
move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
@@ -232,19 +232,22 @@ void arm_forward(){
* Do not call the function if the arm has not been lowered!!
*/
void arm_backward(){
+ if(arm_forward_callable != 1){
+ return;
+ }
- if( (servo_6_pos+20) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
+ if( (servo_6_pos+ARM_FORWARD_DELTA) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
servo_6_pos = SERVO_6_UPPER_LIMIT;
}
else{
- servo_6_pos = servo_6_pos + 20;
+ servo_6_pos = servo_6_pos + ARM_FORWARD_DELTA;
}
- if( servo_4_pos-20 <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
+ if( servo_4_pos-ARM_FORWARD_DELTA <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
servo_4_pos = SERVO_4_LOWER_LIMIT;
}
else{
- servo_4_pos = servo_4_pos - 20;
+ servo_4_pos = servo_4_pos - ARM_FORWARD_DELTA;
}
move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
@@ -263,7 +266,6 @@ void arm_backward(){
*/
void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
if(id == ALL_SERVOS){//Please don't.
- int test = 12;
return;
}
@@ -303,7 +305,6 @@ void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t i
}
}
-
void arm_rotate_90_left(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x00D5,imode);
}
@@ -312,7 +313,6 @@ void arm_rotate_90_right(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x0332,imode);
}
-
/**
* Set counter clockwise limit for servo position
*/
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 7ae85a19..2e35c599 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -26,6 +26,8 @@
#define SERVO_4_UPPER_LIMIT 0x03FF
#define SERVO_4_LOWER_LIMIT 0x0000
+#define ARM_FORWARD_DELTA 20 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
+
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
//TODO test these positions
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 45ffcbba..56d7d201 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -53,41 +53,23 @@ int main(void){
/************************************************************************/
/* TEST BELOW */
/************************************************************************/
- //set_return_delay(1,250,IMODE_WRITE);
- //arm_rotate_90_left(0x00FF,IMODE_WRITE);
- //_delay_ms(5000);
- //lower_arm();
- //move_to_angle();
- /*ping(7);
- b1 = uart_1_getchar();
- b2 = uart_1_getchar();
- b3 = uart_1_getchar();
- b4 = uart_1_getchar();
- b5 = uart_1_getchar();
- b6 = uart_1_getchar();
- */
+
/************************************************************************/
/* TEST ABOVE */
/************************************************************************/
- volatile uint8_t b1, b2, b3,b4,b5,b6,b7,b8;
- move_to_angle(6, 0x00FF, 0x02FF,IMODE_REG_WRITE);
- _delay_ms(5000);
- send_action();
-/* move_to_angle(3, 0x00FF, 358, IMODE_WRITE);
- send_action();
- _delay_ms(6000);
- move_to_angle(3, 0x00FF,205,IMODE_WRITE);
- send_action();
-*/
-// lower_arm();
- /*
+
+
+
+ uint8_t b1, b2, b3,b4,b5,b6,b7,b8;
+
+
while (true) {
b1 = getchar();
b2 = getchar();
b3 = getchar();
translate_instruction(b1,b2,b3);
}
- */
+
return 0;
diff --git a/arm/arm/serial.c b/arm/arm/serial.c
index 477ab851..52ef7ee5 100644
--- a/arm/arm/serial.c
+++ b/arm/arm/serial.c
@@ -24,19 +24,8 @@ char uart_0_getchar(FILE *stream){
}
void uart_1_putchar(char c){
- /*loop_until_bit_is_set(UCSR1A, UDRE1);//Wait until data register is empty and ready to receive new data
- DDRD = 1; //Enable output, disable input
- UCSR1B &= ~(_BV(RXEN1));
- UCSR1B |= _BV(TXEN1);
- */
loop_until_bit_is_set(UCSR1A, UDRE1);
UDR1 = c;
-/*
- loop_until_bit_is_set(UCSR1A, TXC1);// Wait until transmission of data is complete
- DDRD = 0;//Disable output, enable input
- UCSR1B &= ~(_BV(TXEN1));
- UCSR1B |= _BV(RXEN1);
-*/
}
char uart_1_getchar(){
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 557030af..6ca197b1 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -55,9 +55,11 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
set_speed(arg1);
break;
case 8: //set angle
- set_goal_position(arg1, arg2, IMODE_WRITE);
+ move_to_angle(arg1, 0x00FF, arg2, IMODE_REG_WRITE);//arg1 is id arg2 is goal_position
+ send_action();//Always send_action since we might call a pair of servos
break;
case 9: //stop servo
+ torque_enable(arg1,0,IMODE_WRITE);
break;
case 10: //get angle
break;
@@ -69,11 +71,11 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
set_moving_speed(arg1, arg2, IMODE_WRITE);
break;
case 15: //open claw
- move_to_angle(CLAW_ID, arg1, 512, IMODE_WRITE); //TODO check if CLAW_ID and given goal_position is right
+ move_to_angle(CLAW_ID, arg1, 512, IMODE_WRITE);
break;
case 16: //close claw
- move_to_angle(CLAW_ID, arg1, 0, IMODE_WRITE);//TODO check if CLAW_ID and given goal_position is right
- break;
+ move_to_angle(CLAW_ID, arg1, 0, IMODE_WRITE);//Right now the claw will close fully, we probably
+ break; //want it to close halway or something
case 17: // rotate arm left
arm_rotate_90_left(arg1,IMODE_WRITE);
break;
@@ -87,10 +89,11 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
raise_arm();
break;
case 21: //arm forward
+ arm_forward();
break;
case 22: //arm backward
+ arm_backward();
break;
-
case 50: //rotate 90 left
rotate_90_left();
break;
diff --git a/arm/arm/styrmodul.cproj b/arm/arm/styrmodul.cproj
index c85bf47c..22033b1b 100644
--- a/arm/arm/styrmodul.cproj
+++ b/arm/arm/styrmodul.cproj
@@ -47,7 +47,7 @@
<InterfaceName>JTAG</InterfaceName>
</ToolOptions>
<ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
- <ToolNumber>J41800055572</ToolNumber>
+ <ToolNumber>J41800087887</ToolNumber>
<ToolName>Atmel-ICE</ToolName>
</com_atmel_avrdbg_tool_atmelice>
<avrtoolinterface>JTAG</avrtoolinterface>
diff --git a/arm/styrmodul.atsuo b/arm/styrmodul.atsuo
index f3471f7490a994cdc823ed8114541c2524254a5d..53f225e5c900616c47292033fad217f556bf0195 100644
GIT binary patch
delta 3082
zcmb`JdraHc6~})+V}lJg#XJlc62dT)m(9;_JK^CckkF12vUM9t7h_D~f^lk08wV4d
zELk6Ev)Ly~v!qFB*R0#RR-3G;ou;i+5N&I^DU~*9TB}u6rK(b;Nt4(g(YCPfH8wLP
zo%TnM_3^#;+;bnlbMHO(CNA)aOZ-n0BBh4oxG#YwaA$pe{Z2Y5`X$fTQ=_<{^wwXm
zG^}6$6JLPJ<qkR_ep?Qcbyj34y(`wF6k<9q8KM=jp4KGqh`)(CS}oL6ek<EyPc}Wv
zD*jb?kUvCUFREL7D62k<)Cb0a9~=PzFbRGLj)EWvfhoY|_+6wkAPkOyM?eJ3f+&~+
zkAla*aS#J1z&wb91&-UnokV64B*4X2|K0WeiM>PTF1`Qhjgw7}Q&KLFL;7Il^<OvL
z_mb)f%gd)H_y62bc)bY5bjndh%cx9+KY8@iz17}pio|>WcsKYPQMNMekQML>p4m2o
zoTn&SJgmr+6#3dUrj<dX&eujuh827VG>v)M^jb-hCvEd?DlT)TN>>y-Te+${3tB6E
zfL6r<Meli<mYohdS5`|$n)g$O@+)+>t(3m3iKG+CGX(_>e8@;sU%J1_!1MP}Of@8z
zqNCEh&eChD;R3d>>8SbuFD=fiqP4nPnXOQ=MOlAKUJHdXPgy0Pa7K<lNHKR3IT`mX
zl?@mqv(Z>2ly>ZF7a2{coadaJf$QhIoRu@-U(abdS0-;nPLH356OeOe>YQ|S*Aada
z->{eVHMY>fDk;Sqca?n@z4M~`^;^o<)ZEhV?pa^KKCVnJHzq{1tO?Oa8XMIr%jp-*
z1F6;;I;)tc%Vk}u7B{U__fVq6t>DWrl^T#+Hi;gEKho`%o3v^aD%k#mA|ruF&^N8~
zr;A#9L~=1y?O?NAHww{otbLI$n#h=Ql$3>OrgJ|zoTHU>BMmb{57o@9-o9ym>-^~R
z%#+EH+RUnrC~HZtY>;OMh=(WUrm3;VB=yJR^EQ{!meQ@t>(GF0x!i!XrW?*2!)O=w
z4%15VYUP6{uoY%P$GRK&3v3Z&nMKI<-$8F^yEgRIn|hu9SS0*#{A;@pPK81rlT-F4
zjkN8cX@!)Ag}Ri%!`wnq1IA>r<X(`Qkz@>o$LO|Uh}lRUMrn>>#(fH|1=-d-OZ7Ag
z>@+eFApJJ0*=qL~7E;@0*-qgB1^)o?j=}l};(`t_LM$@A@<H{JM=yJ114{?W7nom?
zBX#^Y$SLh(UrYPU#x394F7ic4+1tV2WdMmU64D)|H?^&z_{DAGcaq8~Oa(o=1^njO
zWSMC1@|UT=-$J~5G(0^Q4TfiW<C~7fQVJ%Q&FQikv`(|tpf&Y-`?Yqn+p9Gj3<i_Q
z<g&OdriJBgGtf<j*otLWCO+>nSOuG4b!$B~w?%8RdtF+a*X6~Z&11G$EJmwSzYrIU
zMw@?P!lw<`12(Nu@C({;n{Qk@skhqALSVu^;WO|Y&s@hO=*XPXElx90MQ@u}%6aa#
zcYfRJr6zHu$dhr|;^ud5*+2(Rq3hFtz!{LabFg6^NB#-$BzOv(15blzz<KZ;kh4u8
zzXUFTXTe3l>MkLD4qOKJU&$)3BE1Hl2QOrGFCzUucnMqwKL9U-AA=tO*4Zmae*#_w
zuccquJ;c*n&Gj^_ET)~-Ryx_EQnIL>6BhSs>5Xn<&FAe5a`YXbgtn8G`!w{SP0d>H
zfSneWG7+=P-&5uFf+y%_`^6WD9X5=OjmBo9fsk%A8VSxE>D(2X^@k(V!ST*rUkyZN
zVb)`!Usbe~Tdh8`-)zy_ji!KLvmfdlBfh+1=b&%)Xw)}89r)aqrq8xK#P0Z_Z0}Nc
zICRVx2}Go<r_tbv09U|CQ<!GYKwt(z%I|$-&Nm&5#&U*-_5bgjo41Xe2~x7$HWahW
z7c-P<+feA5E5Qw+BHpQPKJ765?8XNo^*frB>d)^@jpc3uwm_Rg3Del3pS#x5aQx$y
zzkOmE?q1rnXZQef*QU)zCO;;8F_C~xmkI9BoMcX-dLtTIH=?1R`{(5ERrWvpCDJ6j
zdF7+7*WYGw`7@J4F@SfMfwOa3@>y%P9P@^h%agg?+p-PWLr;=K;S|MOi~Jnjb`8m!
zQ0~k#X#5@u3?}>Wh?~VlJ%Ptt6d^i{hgy`B(vGqkPQ<faUI^HvSl@DX;8-&z<Wp4O
zYH6iCo*N{HOXz%0IdzGpBG3N})pT{BT9xOS%q7nH%QRE->%{l>ZTnPqndsPIRt+=T
ztguHUo1^FMA+T=;yT5YMfed@`Y)H_N+!n8^QiBOfT6a=SuLdXFv)v+l8#xs$gl$O6
z@?{zsJbUkh@Z2PxKFps%JdbCPi{KgL<;<Lob7KjeEX6LaEo+BwE6}_e<a}HBZ4{<N
Sc+&2pRd+Wn$#G-<H~kM;7`iL~
delta 2081
zcmb_ddu)?c6u-CYx3$~Xt%W_x9!jO%v+erWn1hbC+qW|5U=N18mW?ron{IAnMHtMi
zsEG>VXb(Y<NTwzRLsUpLY9wK1OC-J+Mna4sLLz7&Mldyi59T>tH#=hd%hQ~m$36Fa
z_x`@$y`TRO_aERV{4&+usi~<47-Nf2@1ATJ;6ZsU&j6kL{^U`gPd)m^c^-#D{zBL(
z|4bf_E}WB$L6sbK2woMJVWJQfyG+43xgNs87jo(e6Y=^~a!4z%V#bF*<D2=*us$Ix
z(3Gv~z>zJe$D%#3I3kZ@+>LrS&`_uTQt{mAyE)A(Uz_-A)$py6XCNS2z@=EL`O3Jb
zRXp>mHDOP6rtasu9|bt3=v80dF>$u1AoIHHk4vAnG+nKM%O$CyrAh^dak&+IiFGQT
zjja{YM@}!BvT2yfip~0x>I9aF8QOCwoD|^juwhp)c|6i7oLn!?$3gX+DJd_)`zkLK
z8C0NB*TQ!O1AMQKha>uCIIdn3@@t-rOK{+fb!-W2mdXg+P<5J}^BNdT_lo%#C&x4l
zh12WeRGdIr5S#X8HgQ@NgtI0^dVxtXqY%!X6sfZ&CYfcyAO)2qktrjVGfox)Wk#WG
zYmd1-<hrj=2A3QqaCG4oVRLkuRZy?fa~bsOilDhd0Sgt`p)s9b7S#B`rSE}j>U?-x
zH4oOS8{w9Ic`&z1;EB|iR_M#EBfJDFCR*T5?&o3xdOB3v6~QkOIWUe0EHh|B+I*u-
zEWns}Ia4MOZhfJ_!eJhP5F-dEd*KOVqr7$+Pp7$!ZDl>o%sScaw7y5psuBJ>{Bkho
zXsVWWO08lA&XyWvLodOJ<+eE7KUm8*aEi1+^8`%DEf6*%F=!^?MKdFG@fAn5B-UV(
zctira`!b_Ftnh<18~T>3psmy%%@x7egZca_xRs}QC%6^PpiL6xIM0CkPh;CORvR(d
z4|i-{QIDP~rbKuQgWWJ_UjgHJ)=m2{+J_nu+Icf;V^1Omt<1-I@qZeRR<<qT`4E|Q
z*45gW{Ogrd-tli!I)e4r#%35Ci_DqAy&tsnUZBGRwKZCBxi-%0WVYuMuBLqw=X>*;
z;+rGC-CRw>so1f!<(*cis!`LuxJP(>yW?UCl-Cr&+R}=c-78_9RIv0smzG%Yx65tw
zRF{=hTTPW^C3ch5Dp^eKYOC91vA8W&<ra^}Q)Pc@$g#3(pxx8e-7&Ci2-X?~V7`<u
z8`>Z9!ir#0t%Tc)^ZDM{EPu!jaUT7gj2ZS<3zSVp(nna2n02}K%}#3btaA<}GU<O<
zA$(Mwk=h=8D~uJ7?LeOnWT3%i3ZM<566p^_?ZK!$6t%CQJwkq<!7jW-&dG+2Tqeab
zb%te&=#<Y2T^`x6&0)WXdxSnhP@bctJAm#I>7EK3ta7BM&o)AbI8WT0^Vq(0x+iCZ
zt{xJJz|Vm(M-!d<=?7@>tmJ}UR6|pRn(hoF{_c_*%YW0elRa(zdcpC$3#Tq#@9(Z$
z{oV^mGA(kjm~Q~LYXu~^)CR-e>@{Zhhat_!BR5*NhK|ybV%bZ67mOKKo61`XmUTHZ
zb}csd|5V>RHA&06x3UAzhK-doQ&8;+R!U+X)(Fu$I`0BtFbsp0!+bm3sq~7u7;9q-
z4r6c_ze`@2SmA(k>IIOY$OD%&22M*_<P1zxa!L9Xteg?THiB;~b7y#3LEn~XaiXF7
xUHVtvM_!vTTFTgs(-QhbaA^*=iIbYn*)qm<qHn-8FAdiCVUzcfSsfLd{s#8vuyz0d
--
GitLab
From 25a49b93439ed7e21b447cdd1a3a13d9510fd5f3 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Fri, 29 Nov 2019 13:17:24 +0100
Subject: [PATCH 10/20] Changed some arm-functions to return a non zero int on
failure
---
arm/arm/arm.c | 29 +++++++++++++++++++++--------
arm/arm/arm.h | 8 ++++----
2 files changed, 25 insertions(+), 12 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 2e1e4841..f39a565c 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -117,11 +117,16 @@ uint8_t get_addr(char name[]){
return 254;//Name not found
}
-void set_led(uint8_t id, uint16_t led_value, uint8_t imode){
+/**
+ * Returns non-zero value on failure
+ * Returns 0 else
+ */
+uint8_t set_led(uint8_t id, uint16_t led_value, uint8_t imode){
if(led_value != 0 && led_value != 1){
- return;
+ return 1;
}
write_two_bytes(id, "LED", led_value, 1, imode);
+ return 0;
}
/**
@@ -201,10 +206,11 @@ void lower_arm(){
/**
* Move arm slightly forward. Meant to be used after lower_arm() has been called!
* Do not call the function if the arm has not been lowered!!
+ * Returns a non-zero value on failure
*/
-void arm_forward(){
+uint8_t arm_forward(){
if(arm_forward_callable != 1){
- return;
+ return 1;
}
if( (servo_6_pos-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
@@ -225,15 +231,18 @@ void arm_forward(){
move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
_delay_ms(100);
send_action();
+ return 0;
}
/**
* Move arm slightly backward. Meant to be used after lower_arm() has been called!
* Do not call the function if the arm has not been lowered!!
+ *
+ * Returns a non-zero value on failure
*/
-void arm_backward(){
+uint8_t arm_backward(){
if(arm_forward_callable != 1){
- return;
+ return 1;
}
if( (servo_6_pos+ARM_FORWARD_DELTA) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
@@ -254,6 +263,7 @@ void arm_backward(){
move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
_delay_ms(100);
send_action();
+ return 0;
}
@@ -263,10 +273,12 @@ void arm_backward(){
* If the servo is paired, the instruction is also sent to its paired servo with its corresponding goal position,
* and their instructions are executed simultaneously.
* If servo is paired, the imode is always IMODE_REG_WRITE, and must be followed by a send_action() in order to execute.
+ *
+ * Returns a non-zero value on failure
*/
-void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
+uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
if(id == ALL_SERVOS){//Please don't.
- return;
+ return 1;
}
uint8_t lower_byte_pos = goal_position& 0xFF;
@@ -303,6 +315,7 @@ void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t i
} else {
send_instruction(id, imode, params, 5);
}
+ return 0;
}
void arm_rotate_90_left(uint16_t speed,uint8_t imode){
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 2e35c599..7bdcf70e 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -42,7 +42,7 @@ uint8_t get_addr(char name[]);
void servo_init();
/* Set led of specified servo. */
-void set_led(uint8_t id, uint16_t led_value, uint8_t imode);
+uint8_t set_led(uint8_t id, uint16_t led_value, uint8_t imode);
/* Set goal position of specified servo. */
void set_goal_position(uint8_t id, uint16_t goal_position, uint8_t imode);
@@ -51,7 +51,7 @@ void set_goal_position(uint8_t id, uint16_t goal_position, uint8_t imode);
void set_moving_speed(uint8_t id, uint16_t speed, uint8_t imode);
/* Performs both goal position and set moving speed of specified servo. */
-void move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode);
+uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode);
/* Ping specified servo. */
void ping(uint8_t id);
@@ -80,7 +80,7 @@ void lower_arm();
void arm_rotate_90_right(uint16_t speed,uint8_t imode);
void arm_rotate_90_left(uint16_t speed,uint8_t imode);
void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
-void arm_forward();
-void arm_backward();
+uint8_t arm_forward();
+uint8_t arm_backward();
#endif // ARM_H_
\ No newline at end of file
--
GitLab
From 77e4c72961b3d08b7e4189e508107f4e20131bcf Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Fri, 29 Nov 2019 13:40:33 +0100
Subject: [PATCH 11/20] merged with styr2
---
arm/arm/main.c | 1 +
arm/arm/steering.c | 49 ++++++++++++++++++++++++++++++++-------------
arm/arm/steering.h | 10 +++++----
arm/arm/styr_komm.c | 20 +++++++++---------
4 files changed, 53 insertions(+), 27 deletions(-)
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 56d7d201..6c252a2e 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -68,6 +68,7 @@ int main(void){
b2 = getchar();
b3 = getchar();
translate_instruction(b1,b2,b3);
+ putchar(255);
}
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index 34f5e973..e03e6b74 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -54,38 +54,59 @@ void pwm_init(){
Equation used is:
u[n] = KP*e[n] + KD*(e[n] –e[n-1])
*/
-void update_ctrl_signal(int16_t new_error){//TODO think of better name for ctrl_signal(styrsignal)
+int16_t update_ctrl_signal(int16_t new_error){//TODO think of better name for ctrl_signal(styrsignal)
int16_t ctrl_signal = KP*new_error + KD*(new_error - prev_error);
prev_error = new_error;
- turn(ctrl_signal);
+ return ctrl_signal;
}
-void turn(int16_t ctrl_signal) {
- // TODO make wheels back if value goes below 0
+void follow_line(int16_t ctrl_signal) {
int16_t new_left_speed = (int16_t) (default_speed) - (ctrl_signal/8);
int16_t new_right_speed = (int16_t) (default_speed) + (ctrl_signal/8);
+
+ // LEFT
if (new_left_speed < 0) {
- set_left_speed(0);
- } else if (new_left_speed > 255) {
- set_left_speed(255);
+ set_left_direction(BACK);
+ } else {
+ set_left_direction(FORWARD);
+ }
+ uint8_t abs_left_dir = 0;
+ if (abs(new_left_speed) > 255) {
+ abs_left_dir = 255;
} else {
- set_left_speed((uint8_t) new_left_speed);
+ abs_left_dir = (uint8_t) (abs(new_left_speed));
}
+ set_left_speed(abs_left_dir);
+
+ // RIGHT
if (new_right_speed < 0) {
- set_right_speed(0);
- } else if (new_right_speed > 255) {
- set_right_speed(255);
+ set_right_direction(BACK);
+ } else {
+ set_right_direction(FORWARD);
+ }
+ uint8_t abs_right_dir = 0;
+ if (abs(new_right_speed) > 255) {
+ abs_right_dir = 255;
} else {
- set_right_speed((uint8_t) new_right_speed);
+ abs_right_dir = (uint8_t) (abs(new_right_speed));
}
+ set_right_speed(abs_right_dir);
+
putchar(default_speed);
putchar(left_speed);
putchar(right_speed);
- //set_left_speed(left_speed+(ctrl_signal/2));
- //set_right_speed(right_speed-(ctrl_signal/2));
}
+void center_line(int16_t ctrl_signal) {
+ if (ctrl_signal < 0) {
+ rotate_right();
+ } else {
+ rotate_left();
+ }
+}
+
+
void set_default_speed(uint8_t duty_threshold) {
default_speed = duty_threshold;
}
diff --git a/arm/arm/steering.h b/arm/arm/steering.h
index 2f79d689..99e53bb5 100644
--- a/arm/arm/steering.h
+++ b/arm/arm/steering.h
@@ -19,13 +19,15 @@ enum Direction{BACK, FORWARD};
#define DRIVE_ONE_BLOCK_DELAY 3400
#define TEST_SPEED 80
-#define KP 2 //Constants for styrreglering
-#define KD 2
+#define KP 4 //Constants for styrreglering
+#define KD 3
void pwm_init();
-void update_ctrl_signal(int16_t new_error);
-void turn(int16_t ctrl_signal);
+int16_t update_ctrl_signal(int16_t new_error);
+void follow_line(int16_t ctrl_signal);
+void center_line(int16_t ctrl_signal);
+
void set_right_speed(uint8_t duty_threshold);
void set_left_speed(uint8_t duty_threshold);
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 6ca197b1..8f8fc063 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -104,18 +104,20 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
small_forward();
break;
case 100:
- // Drive to next node
- break;
+ // Rotate to center line
+ set_speed(120);
+ center_line(update_ctrl_signal((((uint16_t)(arg1)) << 8) + arg2));
+ break;
case 101:
- // Read line data
- update_ctrl_signal((((uint16_t)(arg1)) << 8) + arg2);
- break;
+ // Read line data
+ follow_line(update_ctrl_signal((((uint16_t)(arg1)) << 8) + arg2));
+ break;
case 102:
- // Set default speed
- set_default_speed(arg1);
- break;
-
+ // Set default speed
+ set_default_speed(arg1);
+ break;
default:
break;
}
+
}
\ No newline at end of file
--
GitLab
From b1aca5b040f790358921d63205c0b4b1b1739ed8 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Fri, 29 Nov 2019 15:21:29 +0100
Subject: [PATCH 12/20] Added restriction to the arm rotate
---
arm/arm/arm.c | 66 ++++++++++++++++++++++++++++++++++++---------
arm/arm/arm.h | 6 +++--
arm/arm/styr_komm.c | 7 ++++-
3 files changed, 63 insertions(+), 16 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index f39a565c..487209ae 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -18,7 +18,7 @@ uint16_t servo_6_pos;
uint16_t servo_4_pos;
uint16_t servo_3_pos;
-uint8_t arm_forward_callable = 0; //1 if instruction arm_forward should be callable 0 if not
+uint8_t arm_lowered = 0; //1 if instruction arm_forward should be callable 0 if not
//Array of instruction names. The index of the instruction string is the address of the instruction.
const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
@@ -157,20 +157,53 @@ void torque_enable(uint8_t id, uint16_t enable, uint8_t imode){
write_two_bytes(id,"Torque Enable", enable, 1, imode);
}
-void raise_arm(){
+/**
+ * Disables the torque on all servos except 1 and 8;
+ */
+void disable_torque(){
+ torque_enable(2,0,IMODE_WRITE);//Disable servos 2 and 3 since they don't need to work
+ torque_enable(3,0,IMODE_WRITE);
+ torque_enable(4,0,IMODE_WRITE);
+ torque_enable(5,0,IMODE_WRITE);
+ torque_enable(6,0,IMODE_WRITE);
+ torque_enable(7,0,IMODE_WRITE);
+}
+
+uint8_t raise_arm(){
+ if(arm_lowered != 1){
+ return 1; //Arm is not lowered. Do not raise
+ }
move_to_angle(2,0x00AF,0x031F,IMODE_REG_WRITE);
move_to_angle(5,0x00AF,0x0300,IMODE_REG_WRITE);
move_to_angle(6,0x00AF,0x03FF,IMODE_REG_WRITE);
- _delay_ms(2000);
+ _delay_ms(200);
send_action();
_delay_ms(2000);
- torque_enable(2,0,IMODE_WRITE);//Disable servos 2 and 3 since they don't need to work
- torque_enable(3,0,IMODE_WRITE);
- arm_forward_callable = 0;
-
+ arm_lowered = 0;
+ return 0;
}
+/**
+ * Lowers the robot arm to a position where it should be able to pick up stuff
+ * Takes a direction for which side the body should rotate to first, 1 for right, 0 for left
+ * Note that this means the arm will be lowered to the left side if dir=1 and right side if dir=0
+ *
+ * Returns a non-zero value on failure
+ */
+uint8_t lower_arm(uint8_t dir){
+ if(arm_lowered == 1){
+ return 1;//Arm already lowered
+ }
+
+ if(dir == 1){
+ arm_rotate_90_right(0x00FF, IMODE_WRITE);
+ }
+ else if(dir == 0){
+ arm_rotate_90_left(0x00FF,IMODE_WRITE);
+ }
+ else{
+ return 1;//Bad dir
+ }
-void lower_arm(){
torque_enable(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
torque_enable(3,1,IMODE_WRITE);//we make sure that they are enabled here
_delay_ms(200);
@@ -200,7 +233,8 @@ void lower_arm(){
servo_4_pos = 400;
servo_6_pos = 818;
- arm_forward_callable = 1;
+ arm_lowered = 1;
+ return 0;
}
/**
@@ -209,7 +243,7 @@ void lower_arm(){
* Returns a non-zero value on failure
*/
uint8_t arm_forward(){
- if(arm_forward_callable != 1){
+ if(arm_lowered != 1){
return 1;
}
@@ -241,7 +275,7 @@ uint8_t arm_forward(){
* Returns a non-zero value on failure
*/
uint8_t arm_backward(){
- if(arm_forward_callable != 1){
+ if(arm_lowered != 1){
return 1;
}
@@ -273,13 +307,15 @@ uint8_t arm_backward(){
* If the servo is paired, the instruction is also sent to its paired servo with its corresponding goal position,
* and their instructions are executed simultaneously.
* If servo is paired, the imode is always IMODE_REG_WRITE, and must be followed by a send_action() in order to execute.
+ * The arm can't rotate if it is down.
*
* Returns a non-zero value on failure
*/
uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
- if(id == ALL_SERVOS){//Please don't.
+ if(id == ALL_SERVOS || (id == 1 && arm_lowered)){ // Please don't.
+ // Don't rotate the arm when arm is down
return 1;
- }
+ }
uint8_t lower_byte_pos = goal_position& 0xFF;
uint8_t higher_byte_pos = goal_position >> 8;
@@ -326,6 +362,10 @@ void arm_rotate_90_right(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x0332,imode);
}
+void arm_rotate_default(uint16_t speed,uint8_t imode){
+ move_to_angle(1, speed, 0x01FF,imode);
+}
+
/**
* Set counter clockwise limit for servo position
*/
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 7bdcf70e..5084a7fb 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -75,12 +75,14 @@ void send_instruction(uint8_t id, uint8_t imode, uint8_t *params, uint8_t param_
void set_return_delay(uint8_t id, uint16_t delay, uint8_t imode);
void set_max_torque(uint8_t id, uint16_t torque, uint8_t imode);
void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode);
-void raise_arm();
-void lower_arm();
+uint8_t raise_arm();
+uint8_t lower_arm(uint8_t dir);
void arm_rotate_90_right(uint16_t speed,uint8_t imode);
void arm_rotate_90_left(uint16_t speed,uint8_t imode);
void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
uint8_t arm_forward();
uint8_t arm_backward();
+void disable_torque();
+void arm_rotate_default(uint16_t speed,uint8_t imode);
#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 8f8fc063..ff9d17d9 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -83,7 +83,7 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
arm_rotate_90_right(arg1,IMODE_WRITE);
break;
case 19: //arm down
- lower_arm();
+ lower_arm(arg1);
break;
case 20://arm up
raise_arm();
@@ -103,6 +103,11 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 52: //Small forward
small_forward();
break;
+ case 53:
+ disable_torque();
+ break;
+ case 54:
+ arm_rotate_default(arg1,IMODE_WRITE);
case 100:
// Rotate to center line
set_speed(120);
--
GitLab
From 474404e6149e23f70230496febcaeed1d6611d8c Mon Sep 17 00:00:00 2001
From: jonjo049 <jonjo049@student.liu.se>
Date: Sat, 30 Nov 2019 15:52:07 +0100
Subject: [PATCH 13/20] Minor changes
---
arm/arm/arm.c | 40 +++++++++-----
arm/arm/steering.c | 22 ++++----
arm/arm/styr_komm.c | 23 +++++---
arm/arm/styrmodul.cproj | 120 ++++++++++++++++++++++++----------------
4 files changed, 121 insertions(+), 84 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 487209ae..7f606336 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -152,21 +152,27 @@ void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode){
write_two_bytes(id, "Baud Rate", baud_rate, 1, imode);
}
-//Enable or disable torque for a servo with the given id
-void torque_enable(uint8_t id, uint16_t enable, uint8_t imode){
+void set_torque(uint8_t id, uint16_t enable, uint8_t imode){
write_two_bytes(id,"Torque Enable", enable, 1, imode);
}
/**
- * Disables the torque on all servos except 1 and 8;
+ * Disables the torque on all servos except servos with id's 1 and 8.
+ * This is used after grabbing the item to not exhaust the servos, but since 8 is holding the item and 1 is used for keeping the arm steady these remain powered.
*/
-void disable_torque(){
- torque_enable(2,0,IMODE_WRITE);//Disable servos 2 and 3 since they don't need to work
- torque_enable(3,0,IMODE_WRITE);
- torque_enable(4,0,IMODE_WRITE);
- torque_enable(5,0,IMODE_WRITE);
- torque_enable(6,0,IMODE_WRITE);
- torque_enable(7,0,IMODE_WRITE);
+void disable_all_torque(){
+ for (int i = 2; i < 8; i++) {
+ set_torque(i, 0, IMODE_WRITE);
+ }
+}
+
+/**
+ * Enables the torque on all servos.
+ */
+void enable_all_torque() {
+ for (int i = 1; i <= 8; i++) {
+ set_torque(i, 1, IMODE_WRITE);
+ }
}
uint8_t raise_arm(){
@@ -204,8 +210,8 @@ uint8_t lower_arm(uint8_t dir){
return 1;//Bad dir
}
- torque_enable(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
- torque_enable(3,1,IMODE_WRITE);//we make sure that they are enabled here
+ set_torque(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
+ set_torque(3,1,IMODE_WRITE);//we make sure that they are enabled here
_delay_ms(200);
move_to_angle(6, 0x00FF, 0x03FF,IMODE_WRITE);
@@ -244,7 +250,7 @@ uint8_t lower_arm(uint8_t dir){
*/
uint8_t arm_forward(){
if(arm_lowered != 1){
- return 1;
+ return 1; //Don't call if not lowered
}
if( (servo_6_pos-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
@@ -303,11 +309,11 @@ uint8_t arm_backward(){
/**
* Moves the servo with the given id to a given goal position with the given speed
- * Sends an instruction that sets the speed and the goal position with only one package
+ * These functions can be merged into one function and can be sent using only one package due to the memory configuration in the servo.
* If the servo is paired, the instruction is also sent to its paired servo with its corresponding goal position,
* and their instructions are executed simultaneously.
* If servo is paired, the imode is always IMODE_REG_WRITE, and must be followed by a send_action() in order to execute.
- * The arm can't rotate if it is down.
+ * This function can't be called if the arm is down due to safety reasons.
*
* Returns a non-zero value on failure
*/
@@ -362,6 +368,10 @@ void arm_rotate_90_right(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x0332,imode);
}
+/**
+ * Returns the arm to its "resting position".
+ * Used after item has been grabbed and the car is about to drive.
+ */
void arm_rotate_default(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x01FF,imode);
}
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index e03e6b74..959f1ade 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -6,6 +6,8 @@
*/
#define F_CPU 16000000UL
+
+//Help-functions for accessing and modifying bytes
#define bit_set(port, bit) (port) |= (1<<(bit))
#define bit_clear(port, bit) (port) &= ~(1<<(bit))
#define bit_get(port, bit) (port) & (1<<(bit))
@@ -20,17 +22,15 @@ uint8_t right_speed = 0;//Current speed of the right wheel pair
uint8_t left_speed = 0;
uint8_t default_speed = DEFAULT_SPEED;
-
-
enum Direction left_dir;//Current Direction of the left wheel pair
enum Direction right_dir;
enum Direction DEFAULT_DIR = FORWARD;
-/*
-Fault for "styrreglering"
-The range of the error values are: -32768 to 32767
--32768 < left < 0 < right < 32767
-*/
+/**
+ * Fault for "styrreglering"
+ * The range of the error values are: -32768 to 32767
+ * -32768 < left < 0 < right < 32767
+ **/
int16_t prev_error;
/**
@@ -50,10 +50,10 @@ void pwm_init(){
prev_error = 0;
}
-/*
- Equation used is:
- u[n] = KP*e[n] + KD*(e[n] –e[n-1])
-*/
+/**
+ * Equation used is:
+ * u[n] = KP*e[n] + KD*(e[n] –e[n-1])
+ */
int16_t update_ctrl_signal(int16_t new_error){//TODO think of better name for ctrl_signal(styrsignal)
int16_t ctrl_signal = KP*new_error + KD*(new_error - prev_error);
prev_error = new_error;
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index ff9d17d9..0d694c9c 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -9,6 +9,9 @@
#include "steering.h"
#include "arm.h"
+//This is the return value returned when the pi initially asks to identify this unit.
+#define STYRMODUL_ID 10
+
/**
* Executes an instruction according to the given instruction number and the given arguments
*
@@ -20,7 +23,7 @@
void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
switch (instr) {
case 255: //Identify
- putchar(10);
+ putchar(STYRMODUL_ID);
break;
case 0: //Forward
set_direction(FORWARD);
@@ -30,21 +33,21 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
set_direction(BACK);
set_speed(arg1);
break;
- case 2://Turn left (Noting?)
+ case 2://Turn left
+ set_direction(FORWARD);
set_right_speed(arg1);
set_left_speed(arg1*TURN_CONSTANT);
break;
- case 3: //Turn right (Noting?)
+ case 3: //Turn right
+ set_direction(FORWARD);
set_right_speed(arg1*TURN_CONSTANT);
set_left_speed(arg1);
break;
case 4: //Rotate left
- //Setup directions for rotation. Needs a speed to initiate.
set_speed(arg1);
rotate_left();
break;
case 5: //Rotate right
- //Setup directions for rotation. Needs a speed to initiate.
set_speed(arg1);
rotate_right();
break;
@@ -54,18 +57,21 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 7://Set driving speed
set_speed(arg1);
break;
- case 8: //set angle
+ case 8: //move to angle
move_to_angle(arg1, 0x00FF, arg2, IMODE_REG_WRITE);//arg1 is id arg2 is goal_position
send_action();//Always send_action since we might call a pair of servos
break;
case 9: //stop servo
- torque_enable(arg1,0,IMODE_WRITE);
+ set_torque(arg1,0,IMODE_WRITE);
break;
case 10: //get angle
+ //This cant be used for servo 1 - 6 due to technical difficulties, and has been decided to not be implemented
break;
case 11: //get temperature
+ //This cant be used for servo 1 - 6 due to technical difficulties, and has been decided to not be implemented
break;
case 12: //get voltage
+ //This cant be used for servo 1 - 6 due to technical difficulties, and has been decided to not be implemented
break;
case 13: //set servo speed
set_moving_speed(arg1, arg2, IMODE_WRITE);
@@ -104,7 +110,7 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
small_forward();
break;
case 53:
- disable_torque();
+ disable_all_torque();
break;
case 54:
arm_rotate_default(arg1,IMODE_WRITE);
@@ -124,5 +130,4 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
default:
break;
}
-
}
\ No newline at end of file
diff --git a/arm/arm/styrmodul.cproj b/arm/arm/styrmodul.cproj
index 22033b1b..f3856712 100644
--- a/arm/arm/styrmodul.cproj
+++ b/arm/arm/styrmodul.cproj
@@ -2,7 +2,7 @@
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup>
<SchemaVersion>2.0</SchemaVersion>
- <ProjectVersion>6.2</ProjectVersion>
+ <ProjectVersion>7.0</ProjectVersion>
<ToolchainName>com.Atmel.AVRGCC8.C</ToolchainName>
<ProjectGuid>{dafab8c5-acd9-4d74-b97a-bdb50cb799dc}</ProjectGuid>
<avrdevice>ATmega1284P</avrdevice>
@@ -55,59 +55,81 @@
<PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
<ToolchainSettings>
<AvrGcc>
- <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
- <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
- <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
- <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
- <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
- <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
- <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
- <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
- <avrgcc.compiler.symbols.DefSymbols>
- <ListValues>
- <Value>NDEBUG</Value>
- </ListValues>
- </avrgcc.compiler.symbols.DefSymbols>
- <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
- <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
- <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
- <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
- <avrgcc.linker.libraries.Libraries>
- <ListValues>
- <Value>libm</Value>
- </ListValues>
- </avrgcc.linker.libraries.Libraries>
- </AvrGcc>
+ <avrgcc.common.Device>-mmcu=atmega1284p -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\gcc\dev\atmega1284p"</avrgcc.common.Device>
+ <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
+ <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+ <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+ <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+ <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+ <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+ <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+ <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+ <avrgcc.compiler.symbols.DefSymbols>
+ <ListValues>
+ <Value>NDEBUG</Value>
+ </ListValues>
+ </avrgcc.compiler.symbols.DefSymbols>
+ <avrgcc.compiler.directories.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.compiler.directories.IncludePaths>
+ <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
+ <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+ <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+ <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+ <avrgcc.linker.libraries.Libraries>
+ <ListValues>
+ <Value>libm</Value>
+ </ListValues>
+ </avrgcc.linker.libraries.Libraries>
+ <avrgcc.assembler.general.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.assembler.general.IncludePaths>
+</AvrGcc>
</ToolchainSettings>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
<ToolchainSettings>
<AvrGcc>
- <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
- <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
- <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
- <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
- <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
- <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
- <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
- <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
- <avrgcc.compiler.symbols.DefSymbols>
- <ListValues>
- <Value>DEBUG</Value>
- </ListValues>
- </avrgcc.compiler.symbols.DefSymbols>
- <avrgcc.compiler.optimization.level>Optimize (-O1)</avrgcc.compiler.optimization.level>
- <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
- <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
- <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
- <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
- <avrgcc.linker.libraries.Libraries>
- <ListValues>
- <Value>libm</Value>
- </ListValues>
- </avrgcc.linker.libraries.Libraries>
- <avrgcc.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcc.assembler.debugging.DebugLevel>
- </AvrGcc>
+ <avrgcc.common.Device>-mmcu=atmega1284p -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\gcc\dev\atmega1284p"</avrgcc.common.Device>
+ <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
+ <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+ <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+ <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+ <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+ <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+ <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+ <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+ <avrgcc.compiler.symbols.DefSymbols>
+ <ListValues>
+ <Value>DEBUG</Value>
+ </ListValues>
+ </avrgcc.compiler.symbols.DefSymbols>
+ <avrgcc.compiler.directories.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.compiler.directories.IncludePaths>
+ <avrgcc.compiler.optimization.level>Optimize (-O1)</avrgcc.compiler.optimization.level>
+ <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+ <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+ <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
+ <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+ <avrgcc.linker.libraries.Libraries>
+ <ListValues>
+ <Value>libm</Value>
+ </ListValues>
+ </avrgcc.linker.libraries.Libraries>
+ <avrgcc.assembler.general.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.assembler.general.IncludePaths>
+ <avrgcc.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcc.assembler.debugging.DebugLevel>
+</AvrGcc>
</ToolchainSettings>
</PropertyGroup>
<ItemGroup>
--
GitLab
From ebfd7e3b4412f334c05a854d69eddb3cbb850583 Mon Sep 17 00:00:00 2001
From: Jonatan Jonsson <jonjo049@student.liu.se>
Date: Mon, 2 Dec 2019 15:02:32 +0100
Subject: [PATCH 14/20] added increment and decrement
---
arm/arm/arm.c | 69 +++++++++++++++++++++++++++++++++++++++++++++
arm/arm/arm.h | 6 +++-
arm/arm/main.c | 38 ++++++++++++++++++++++---
arm/arm/styr_komm.c | 14 +++++++--
4 files changed, 120 insertions(+), 7 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 7f606336..a40e0d88 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -18,6 +18,54 @@ uint16_t servo_6_pos;
uint16_t servo_4_pos;
uint16_t servo_3_pos;
+uint16_t servo_angle[8];
+
+/*
+
+ set_cw_limit(1,0x0000,IMODE_WRITE);
+ set_ccw_limit(1,0x03FF,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(2,0x0000,IMODE_WRITE);//TODO check limits
+ set_ccw_limit(2,0x03FF,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(3,0x0000,IMODE_WRITE);//TODO check limits
+ set_ccw_limit(3,0x03FF,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(4,0x0000,IMODE_WRITE);//TODO check limits
+ set_ccw_limit(4,0x03FF,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(5,0x0000,IMODE_WRITE);//TODO check limits
+ set_ccw_limit(5,0x0330,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(6,0x00FF,IMODE_WRITE);
+ set_ccw_limit(6,0x0330,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(7,0x0000,IMODE_WRITE);
+ set_ccw_limit(7,0x03FF,IMODE_WRITE);
+ _delay_ms(100);
+ set_cw_limit(8,0x0000,IMODE_WRITE);
+ set_ccw_limit(8,0x03FF,IMODE_WRITE);
+
+*/
+
+
+uint16_t servo_cw_limits[8] = {0,
+ 0,
+ 0,
+ 0,
+ 0,
+ 0x00FF,
+ 0,
+ 0};
+uint16_t servo_ccw_limits[8] = {0x03FF,
+ 0x03FF,
+ 0x03FF,
+ 0x03FF,
+ 0x0330,
+ 0x0330,
+ 0x03FF,
+ 0x03FF};
+
uint8_t arm_lowered = 0; //1 if instruction arm_forward should be callable 0 if not
//Array of instruction names. The index of the instruction string is the address of the instruction.
@@ -202,9 +250,11 @@ uint8_t lower_arm(uint8_t dir){
if(dir == 1){
arm_rotate_90_right(0x00FF, IMODE_WRITE);
+ servo_angle[0] = 0x0332;
}
else if(dir == 0){
arm_rotate_90_left(0x00FF,IMODE_WRITE);
+ servo_angle[0] = 0x00D5;
}
else{
return 1;//Bad dir
@@ -233,12 +283,22 @@ uint8_t lower_arm(uint8_t dir){
move_to_angle(6, 0x00FF, 818,IMODE_REG_WRITE);
move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);//Open claw
+ move_to_angle(7, 0x00FF, 0x01FF, IMODE_REG_WRITE);//Just for default value, this is never changed
send_action();
servo_3_pos = 700;
servo_4_pos = 400;
servo_6_pos = 818;
+ //Setup angles for manual arm steering
+ servo_angle[1] = 0x03FF - 700;
+ servo_angle[2] = 700;
+ servo_angle[3] = 400;
+ servo_angle[4] = 0x03FF - 400;
+ servo_angle[5] = 818;
+ servo_angle[6] = 0x01FF;
+ servo_angle[7] = 0x01FF;
+
arm_lowered = 1;
return 0;
}
@@ -368,6 +428,15 @@ void arm_rotate_90_right(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x0332,imode);
}
+uint8_t increment_servo(uint8_t id, int8_t delta_angle) {
+ uint8_t servo_angle_temp = servo_angle[id-1] + delta_angle;
+ if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
+ return 1;
+ }
+ move_to_angle(id, 0x00FF, servo_angle_temp, IMODE_WRITE);
+ return 0;
+}
+
/**
* Returns the arm to its "resting position".
* Used after item has been grabbed and the car is about to drive.
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 5084a7fb..1b035a58 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -30,6 +30,8 @@
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
+#define INCREMENT_CONSTANT 10 //Check this
+
//TODO test these positions
#define SERVO_90_RIGHT 205 //Position for 90 degrees to the right
#define SERVO_90_LEFT 818 //Position for 90 degrees to the left
@@ -82,7 +84,9 @@ void arm_rotate_90_left(uint16_t speed,uint8_t imode);
void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
uint8_t arm_forward();
uint8_t arm_backward();
-void disable_torque();
void arm_rotate_default(uint16_t speed,uint8_t imode);
+void enable_all_torque();
+uint8_t increment_servo(uint8_t id, int8_t delta_angle);
+void disable_all_torque();
#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 6c252a2e..9fe459d7 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -46,13 +46,43 @@ int main(void){
if (!cold()) {
return 2;
}
-
- // init
- servo_init();
/************************************************************************/
/* TEST BELOW */
/************************************************************************/
+ /**lower_arm(0);
+ _delay_ms(10000);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ increment_servo(8, 20);
+ _delay_ms(2000);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ increment_servo(8, -20);
+ _delay_ms(2000);
+ raise_arm();
+ arm_rotate_default(0x00FF, IMODE_WRITE);
+ _delay_ms(3000);
+ disable_all_torque();
+ */
/************************************************************************/
/* TEST ABOVE */
@@ -62,7 +92,6 @@ int main(void){
uint8_t b1, b2, b3,b4,b5,b6,b7,b8;
-
while (true) {
b1 = getchar();
b2 = getchar();
@@ -83,6 +112,7 @@ bool cold(){
uart_init();
uart_arm_init();
pwm_init();
+ servo_init();
stdout = &uart_output;
stdin = &uart_input;
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 0d694c9c..3f4f91ea 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -109,11 +109,21 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 52: //Small forward
small_forward();
break;
- case 53:
+ case 53: //Disable all torque
disable_all_torque();
break;
- case 54:
+ case 55: //Enable all torque
+ enable_all_torque();
+ break;
+ case 54: //Rotate default
arm_rotate_default(arg1,IMODE_WRITE);
+ break;
+ case 56: //Increment servo
+ increment_servo(arg1, INCREMENT_CONSTANT);
+ break;
+ case 57: //Decrement servo
+ increment_servo(arg1, -INCREMENT_CONSTANT);
+ break;
case 100:
// Rotate to center line
set_speed(120);
--
GitLab
From b793f02ac0f76f126defa45ef26f011ce380969a Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Tue, 3 Dec 2019 17:02:59 +0100
Subject: [PATCH 15/20] Fixed some bugs in the arm
---
arm/arm/arm.c | 92 ++++++++++++------------
arm/arm/arm.h | 5 +-
arm/arm/main.c | 11 +++
arm/arm/styr_komm.c | 2 +-
arm/arm/styrmodul6_2.cproj | 143 +++++++++++++++++++++++++++++++++++++
arm/styrmodul.atsuo | Bin 19456 -> 19456 bytes
6 files changed, 202 insertions(+), 51 deletions(-)
create mode 100644 arm/arm/styrmodul6_2.cproj
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index a40e0d88..6072ffeb 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -14,10 +14,6 @@
#include "arm.h"
#include "serial.h"
-uint16_t servo_6_pos;
-uint16_t servo_4_pos;
-uint16_t servo_3_pos;
-
uint16_t servo_angle[8];
/*
@@ -57,13 +53,13 @@ uint16_t servo_cw_limits[8] = {0,
0x00FF,
0,
0};
-uint16_t servo_ccw_limits[8] = {0x03FF,
- 0x03FF,
- 0x03FF,
- 0x03FF,
- 0x0330,
- 0x0330,
- 0x03FF,
+uint16_t servo_ccw_limits[8] = {0x03FF,//1
+ 0x03FF,//2
+ 0x03FF,//3
+ 0x03FF,//4
+ 0x03FF,//5
+ 0x0330,//6
+ 0x03FF,//7
0x03FF};
uint8_t arm_lowered = 0; //1 if instruction arm_forward should be callable 0 if not
@@ -271,10 +267,6 @@ uint8_t lower_arm(uint8_t dir){
send_action();
_delay_ms(2000);
- move_to_angle(3, 0x00FF, 550, IMODE_REG_WRITE);
- send_action();
- _delay_ms(2000);
-
move_to_angle(4, 0x00FF, 400, IMODE_REG_WRITE);
move_to_angle(3, 0x00A0, 700, IMODE_REG_WRITE);
move_to_angle(8,0x00FF,0x01FF,IMODE_REG_WRITE);
@@ -286,19 +278,6 @@ uint8_t lower_arm(uint8_t dir){
move_to_angle(7, 0x00FF, 0x01FF, IMODE_REG_WRITE);//Just for default value, this is never changed
send_action();
- servo_3_pos = 700;
- servo_4_pos = 400;
- servo_6_pos = 818;
-
- //Setup angles for manual arm steering
- servo_angle[1] = 0x03FF - 700;
- servo_angle[2] = 700;
- servo_angle[3] = 400;
- servo_angle[4] = 0x03FF - 400;
- servo_angle[5] = 818;
- servo_angle[6] = 0x01FF;
- servo_angle[7] = 0x01FF;
-
arm_lowered = 1;
return 0;
}
@@ -313,22 +292,22 @@ uint8_t arm_forward(){
return 1; //Don't call if not lowered
}
- if( (servo_6_pos-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
- servo_6_pos = SERVO_6_LOWER_LIMIT;
+ if( (servo_angle[5]-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
+ servo_angle[5] = SERVO_6_LOWER_LIMIT;
}
else{
- servo_6_pos = servo_6_pos - ARM_FORWARD_DELTA;
+ servo_angle[5] = servo_angle[5] - ARM_FORWARD_DELTA;
}
- if( (servo_4_pos+ARM_FORWARD_DELTA) >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
- servo_4_pos = SERVO_4_UPPER_LIMIT;
+ if( (servo_angle[3] + ARM_FORWARD_DELTA) >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
+ servo_angle[3] = SERVO_4_UPPER_LIMIT;
}
else{
- servo_4_pos = servo_4_pos + ARM_FORWARD_DELTA;
+ servo_angle[3] = servo_angle[3]+ ARM_FORWARD_DELTA;
}
- move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
- move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
+ move_to_angle(6, 0x00FF, servo_angle[5], IMODE_REG_WRITE);
+ move_to_angle(4, 0x00FF, servo_angle[3], IMODE_REG_WRITE);
_delay_ms(100);
send_action();
return 0;
@@ -345,22 +324,22 @@ uint8_t arm_backward(){
return 1;
}
- if( (servo_6_pos+ARM_FORWARD_DELTA) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
- servo_6_pos = SERVO_6_UPPER_LIMIT;
+ if( (servo_angle[5]+ARM_FORWARD_DELTA) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
+ servo_angle[5] = SERVO_6_UPPER_LIMIT;
}
else{
- servo_6_pos = servo_6_pos + ARM_FORWARD_DELTA;
+ servo_angle[5] = servo_angle[5] + ARM_FORWARD_DELTA;
}
- if( servo_4_pos-ARM_FORWARD_DELTA <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
- servo_4_pos = SERVO_4_LOWER_LIMIT;
+ if( servo_angle[3]-ARM_FORWARD_DELTA <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
+ servo_angle[3] = SERVO_4_LOWER_LIMIT;
}
else{
- servo_4_pos = servo_4_pos - ARM_FORWARD_DELTA;
+ servo_angle[3] = servo_angle[3] - ARM_FORWARD_DELTA;
}
- move_to_angle(6, 0x00FF, servo_6_pos, IMODE_REG_WRITE);
- move_to_angle(4, 0x00FF, servo_4_pos, IMODE_REG_WRITE);
+ move_to_angle(6, 0x00FF, servo_angle[5], IMODE_REG_WRITE);
+ move_to_angle(4, 0x00FF, servo_angle[3], IMODE_REG_WRITE);
_delay_ms(100);
send_action();
return 0;
@@ -381,7 +360,7 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
if(id == ALL_SERVOS || (id == 1 && arm_lowered)){ // Please don't.
// Don't rotate the arm when arm is down
return 1;
- }
+ }
uint8_t lower_byte_pos = goal_position& 0xFF;
uint8_t higher_byte_pos = goal_position >> 8;
@@ -392,6 +371,7 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos, higher_byte_pos, lower_byte_speed, higher_byte_speed};
if (id == 2 || id == 3 || id == 4 || id == 5){//Servo-pairs (2 and 3) and (4 and 5) should always move together with "opposite" goal_position
+ servo_angle[id-1] = goal_position; //Update current servo angle for given servo id
send_instruction(id, IMODE_REG_WRITE, params, 5);
_delay_ms(200);
uint16_t goal_position_2 = MAX_GOAL_POSITION - goal_position;
@@ -400,21 +380,26 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
uint8_t params[5] = {get_addr("Goal Position"), lower_byte_pos_2, higher_byte_pos_2, lower_byte_speed, higher_byte_speed};
switch (id) {
case 2:
- send_instruction(3, IMODE_REG_WRITE, params, 5);
+ servo_angle[2] = goal_position_2; //Update servo angle for other servo in pair
+ send_instruction(3, IMODE_REG_WRITE, params, 5);//Move other servo in pair
break;
case 3:
+ servo_angle[1] = goal_position_2;
send_instruction(2, IMODE_REG_WRITE, params, 5);
break;
case 4:
+ servo_angle[4] = goal_position_2;
send_instruction(5, IMODE_REG_WRITE, params, 5);
break;
case 5:
+ servo_angle[3] = goal_position_2;
send_instruction(4, IMODE_REG_WRITE, params, 5);
break;
default:
break;
}
} else {
+ servo_angle[id-1] = goal_position;
send_instruction(id, imode, params, 5);
}
return 0;
@@ -428,8 +413,19 @@ void arm_rotate_90_right(uint16_t speed,uint8_t imode){
move_to_angle(1, speed, 0x0332,imode);
}
-uint8_t increment_servo(uint8_t id, int8_t delta_angle) {
- uint8_t servo_angle_temp = servo_angle[id-1] + delta_angle;
+uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
+ volatile uint16_t servo_angle_temp = servo_angle[id-1] + delta_angle;
+ volatile test = 1;
+ if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
+ return 1;
+ }
+ move_to_angle(id, 0x00FF, servo_angle_temp, IMODE_WRITE);
+ return 0;
+}
+
+uint8_t decrement_servo(uint8_t id, uint16_t delta_angle) {
+ volatile uint16_t servo_angle_temp = servo_angle[id-1] - delta_angle;
+ volatile test = 1;
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
return 1;
}
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 1b035a58..1dbcf880 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -26,7 +26,7 @@
#define SERVO_4_UPPER_LIMIT 0x03FF
#define SERVO_4_LOWER_LIMIT 0x0000
-#define ARM_FORWARD_DELTA 20 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
+#define ARM_FORWARD_DELTA 10 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
@@ -86,7 +86,8 @@ uint8_t arm_forward();
uint8_t arm_backward();
void arm_rotate_default(uint16_t speed,uint8_t imode);
void enable_all_torque();
-uint8_t increment_servo(uint8_t id, int8_t delta_angle);
+uint8_t increment_servo(uint8_t id, uint16_t delta_angle);
void disable_all_torque();
+uint8_t decrement_servo(uint8_t id, uint16_t delta_angle);
#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 9fe459d7..b73944e8 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -89,6 +89,17 @@ int main(void){
/************************************************************************/
+ lower_arm(0);
+ _delay_ms(8000);
+ for (int i=0; i<5; i++)
+ {
+ increment_servo(8,50);
+ _delay_ms(500);
+ decrement_servo(8,30);
+ _delay_ms(500);
+ }
+
+ raise_arm();
uint8_t b1, b2, b3,b4,b5,b6,b7,b8;
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 3f4f91ea..f3dd75a4 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -122,7 +122,7 @@ void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
increment_servo(arg1, INCREMENT_CONSTANT);
break;
case 57: //Decrement servo
- increment_servo(arg1, -INCREMENT_CONSTANT);
+ decrement_servo(arg1, INCREMENT_CONSTANT);
break;
case 100:
// Rotate to center line
diff --git a/arm/arm/styrmodul6_2.cproj b/arm/arm/styrmodul6_2.cproj
new file mode 100644
index 00000000..22033b1b
--- /dev/null
+++ b/arm/arm/styrmodul6_2.cproj
@@ -0,0 +1,143 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+ <PropertyGroup>
+ <SchemaVersion>2.0</SchemaVersion>
+ <ProjectVersion>6.2</ProjectVersion>
+ <ToolchainName>com.Atmel.AVRGCC8.C</ToolchainName>
+ <ProjectGuid>{dafab8c5-acd9-4d74-b97a-bdb50cb799dc}</ProjectGuid>
+ <avrdevice>ATmega1284P</avrdevice>
+ <avrdeviceseries>none</avrdeviceseries>
+ <OutputType>Executable</OutputType>
+ <Language>C</Language>
+ <OutputFileName>$(MSBuildProjectName)</OutputFileName>
+ <OutputFileExtension>.elf</OutputFileExtension>
+ <OutputDirectory>$(MSBuildProjectDirectory)\$(Configuration)</OutputDirectory>
+ <AssemblyName>arm</AssemblyName>
+ <Name>styrmodul</Name>
+ <RootNamespace>arm</RootNamespace>
+ <ToolchainFlavour>Native</ToolchainFlavour>
+ <KeepTimersRunning>true</KeepTimersRunning>
+ <OverrideVtor>false</OverrideVtor>
+ <CacheFlash>true</CacheFlash>
+ <ProgFlashFromRam>true</ProgFlashFromRam>
+ <RamSnippetAddress>0x20000000</RamSnippetAddress>
+ <UncachedRange />
+ <preserveEEPROM>true</preserveEEPROM>
+ <OverrideVtorValue>exception_table</OverrideVtorValue>
+ <BootSegment>2</BootSegment>
+ <eraseonlaunchrule>0</eraseonlaunchrule>
+ <AsfFrameworkConfig>
+ <framework-data xmlns="">
+ <options />
+ <configurations />
+ <files />
+ <documentation help="" />
+ <offline-documentation help="" />
+ <dependencies>
+ <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.19.0" />
+ </dependencies>
+ </framework-data>
+ </AsfFrameworkConfig>
+ <avrtool>com.atmel.avrdbg.tool.atmelice</avrtool>
+ <com_atmel_avrdbg_tool_atmelice>
+ <ToolOptions>
+ <InterfaceProperties>
+ <JtagDbgClock>200000</JtagDbgClock>
+ </InterfaceProperties>
+ <InterfaceName>JTAG</InterfaceName>
+ </ToolOptions>
+ <ToolType>com.atmel.avrdbg.tool.atmelice</ToolType>
+ <ToolNumber>J41800087887</ToolNumber>
+ <ToolName>Atmel-ICE</ToolName>
+ </com_atmel_avrdbg_tool_atmelice>
+ <avrtoolinterface>JTAG</avrtoolinterface>
+ </PropertyGroup>
+ <PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
+ <ToolchainSettings>
+ <AvrGcc>
+ <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
+ <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+ <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+ <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+ <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+ <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+ <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+ <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+ <avrgcc.compiler.symbols.DefSymbols>
+ <ListValues>
+ <Value>NDEBUG</Value>
+ </ListValues>
+ </avrgcc.compiler.symbols.DefSymbols>
+ <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
+ <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+ <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+ <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+ <avrgcc.linker.libraries.Libraries>
+ <ListValues>
+ <Value>libm</Value>
+ </ListValues>
+ </avrgcc.linker.libraries.Libraries>
+ </AvrGcc>
+ </ToolchainSettings>
+ </PropertyGroup>
+ <PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
+ <ToolchainSettings>
+ <AvrGcc>
+ <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
+ <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+ <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+ <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+ <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+ <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+ <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+ <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+ <avrgcc.compiler.symbols.DefSymbols>
+ <ListValues>
+ <Value>DEBUG</Value>
+ </ListValues>
+ </avrgcc.compiler.symbols.DefSymbols>
+ <avrgcc.compiler.optimization.level>Optimize (-O1)</avrgcc.compiler.optimization.level>
+ <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+ <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+ <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
+ <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+ <avrgcc.linker.libraries.Libraries>
+ <ListValues>
+ <Value>libm</Value>
+ </ListValues>
+ </avrgcc.linker.libraries.Libraries>
+ <avrgcc.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcc.assembler.debugging.DebugLevel>
+ </AvrGcc>
+ </ToolchainSettings>
+ </PropertyGroup>
+ <ItemGroup>
+ <Compile Include="arm.c">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="arm.h">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="main.c">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="serial.h">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="serial.c">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="steering.c">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="steering.h">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="styr_komm.c">
+ <SubType>compile</SubType>
+ </Compile>
+ <Compile Include="styr_komm.h">
+ <SubType>compile</SubType>
+ </Compile>
+ </ItemGroup>
+ <Import Project="$(AVRSTUDIO_EXE_PATH)\\Vs\\Compiler.targets" />
+</Project>
\ No newline at end of file
diff --git a/arm/styrmodul.atsuo b/arm/styrmodul.atsuo
index 53f225e5c900616c47292033fad217f556bf0195..98a9e2ea0e28d9564a780893d134fa3c68f3d144 100644
GIT binary patch
delta 1080
zcmYk5Ur19?9LLYmySen<HI?c7)8_QfO`U({pZt%T8&Yb_*$^TLC5)m7s%wZ5?XicT
zwl9PwJr+tY<$dT;!5#tw^+5=H$fAT^<ATyd^*id|{x090^ZT6dIrn#eoTV0cYJtBS
zmAD07$zyi}YGOZ@cX%#iELD$r={(l`A+$&m4y6%=<q?-i|0u96sToDWoMq(5WLQ3j
z{}OMd^)^yFse^Qu)Jf_hiFKh~(_G{Ol!r-~ys0vgXJhijcSVfpM5#JMlWU|UQr0BK
zo5DcjGaA#0QO8y1{;S``zlN437ueLa{|nM`iypQ~IkNaP|H*Fg3DEN63eO=@Z>ENz
z;^cBO)e4ff3KP(cf(?#?KqO<2Vm&_u&7Q<|EC|ECB|FJzHJRmVN+w7mkWh#5Q!T<<
z#}2}3Tu&9oEV74I>?HAGQJij+#B-19C^KPb<zupoK#2+JM)?+(P7yh-pmIwV$5fdt
z6)lvUCW**r!pj(^hXU!&5W?;^`0jbkdvV8|z&lSX_WIf@I$1Y8F&1SHSrZ#)Gwc>=
zn%$#ak*a}=SCb#aB14hEu4qSNB#M-`5|jNl>`AWNX?7Q@UMG^?;7auxM@e-GYc(l6
zRo$X*cpWIhff~Uj$2cxDj-t!w#;(tYuc{S`)E-yBnW>E`+(9LllCqtFRvX8<6tad0
z?sNR9o#)+{^<T7kO)gp73CfQG-xMbX1JzhnB53d__z(y~ag1WP?mp(~6ioPn2nWNN
z`q)wDYG_yXb-YKPt$)PN^br#haf97v8a3oP&Bd)^dSS4cW2~?RE516sXx!!fm~D#V
zV?#R*n>Iwby3xFuqcD}+wWRFvFj_Yi?jurXqPo#0Thj>_Ts`exVHNS54CcScOUf4o
zODBps{dg?PB72{>%vbu(S@Fpqgf=)P2Z&aZ#2H8T-XLfr5Hv$AnzR*|fNn&SR$HN&
IXv0tVe|5r<7XSbN
delta 1344
zcmY+@Z)_7~90%~{*>2r!yZ*UBH=Rqny0&zq9lZe#JEpx}1+kb8Y)mv${!X3BUpvGi
z+o2Fm^ab=sV2X+$;2&Ta9*Mql2{F-Vq!4%oA>j>gct_5}n7H}9a!YP^`SkZZ-``#D
zdiPYH;Q9ogS}X8&3JF0^<<5!V?Ae>VmN7QHm`cJL(!2^K1%dt%a&A70&FWc|t_k6?
zTcEvlx#~e?-Jj_V==H)<SO&}C3Fw0r@Fb+*LzuyL4A}LMUCZ?w$6vzuWqUqk*H`Vj
z5p@%6hAr?q48v>i9lQY}unpb<>l%kqx5EzD3A<o7yaPoTg+1^t>}5=0?_p4aeegc)
zhcOrj9gbc6Z^`Y_n%pN7x9|Tt*7*?~k&;GQ6l$b;bR#hLc&ONzC(Waf*_d~;26Woy
zb(;!e$PpN3(;uArVrto=J$%+dN0*A8g6MY!rl&$#Z&G|5;fp|*W3SMI@Om2Z-86JL
zTPKM~J&schvn5|!eOtx0L8$|smU-Q_3YV;#Z*rt$wpXM_5y+0vpTy*#gb!?uYz(6Y
zcATbG_fa}<uGWj2c|OqU9>!a;p78(^S(J7K^0ob}A9Wo~2YcyW;A48Tqa~AI8LW_H
zSTzYeeuZUO5+jv8gRzQH61xf-)=!eHs_q(&-p_j2*6MpBIorMP49}*L*!N}CRcdC1
zwmhL~q<0i)$S(>?R!NjY?Y_IknbX=&6?w9aef_(4_6nB$LeeE44Tc07h%M&Z3_Uc>
zX}VJ<Mb6XCh>PwhE?OIIrh*)|=k=8H$#f~K(&0#ej&{X~%PA@)f;1V4l%pD5X&oRv
zinr*)uYUyQmV`^e<VFqs8!hpE8eQ}P{U*d^Yn>vmZo|7rHoZ*;x)#v=h@Yl>Uh>9b
z^ov`ilemXNv2=Buk(){G+C%Fb(&g@~B;eF;n7a&LX^eJ{^QNbeZLNMSqazJFXnXuR
z{Ty3OW07Y1EOCoJO`|<|=`kEp<2*Y}fA{?La0%8qa>HD5wZ_fEh!#0wGJCJOSrkdk
zks9Vw$>l$Q<O3i$2-Yv1A%CiG<S1sx;h3FHpnd_L!wEb467?%M38(DrH0n2S2F}7c
z_!fSE@4>pldDIJV5hjgOsT}`t<+Coa3)8tLM8(XY_3t4wGbnYU5uex83Zlyq7^YTm
MiLK70_}r)be@rK*xBvhE
--
GitLab
From 78e144170faeb6b29fc224a0401cdb65b9da1742 Mon Sep 17 00:00:00 2001
From: Jonatan Jonsson <jonjo049@student.liu.se>
Date: Wed, 4 Dec 2019 10:53:59 +0100
Subject: [PATCH 16/20] small testing of limits, changed some
---
arm/arm/arm.c | 38 ++++---------------------
arm/arm/main.c | 57 ++++++++++----------------------------
arm/arm/styrmodul6_2.cproj | 1 +
3 files changed, 21 insertions(+), 75 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 6072ffeb..a5b31321 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -16,40 +16,12 @@
uint16_t servo_angle[8];
-/*
-
- set_cw_limit(1,0x0000,IMODE_WRITE);
- set_ccw_limit(1,0x03FF,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(2,0x0000,IMODE_WRITE);//TODO check limits
- set_ccw_limit(2,0x03FF,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(3,0x0000,IMODE_WRITE);//TODO check limits
- set_ccw_limit(3,0x03FF,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(4,0x0000,IMODE_WRITE);//TODO check limits
- set_ccw_limit(4,0x03FF,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(5,0x0000,IMODE_WRITE);//TODO check limits
- set_ccw_limit(5,0x0330,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(6,0x00FF,IMODE_WRITE);
- set_ccw_limit(6,0x0330,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(7,0x0000,IMODE_WRITE);
- set_ccw_limit(7,0x03FF,IMODE_WRITE);
- _delay_ms(100);
- set_cw_limit(8,0x0000,IMODE_WRITE);
- set_ccw_limit(8,0x03FF,IMODE_WRITE);
-
-*/
-
uint16_t servo_cw_limits[8] = {0,
0,
0,
- 0,
- 0,
+ 0x01FF,//4
+ 0x01FF,//5
0x00FF,
0,
0};
@@ -261,11 +233,11 @@ uint8_t lower_arm(uint8_t dir){
_delay_ms(200);
move_to_angle(6, 0x00FF, 0x03FF,IMODE_WRITE);
- _delay_ms(2000);
+ _delay_ms(900);
move_to_angle(4, 0x00FF, 300, IMODE_REG_WRITE);
send_action();
- _delay_ms(2000);
+ _delay_ms(700);
move_to_angle(4, 0x00FF, 400, IMODE_REG_WRITE);
move_to_angle(3, 0x00A0, 700, IMODE_REG_WRITE);
@@ -419,7 +391,7 @@ uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
return 1;
}
- move_to_angle(id, 0x00FF, servo_angle_temp, IMODE_WRITE);
+ move_to_angle(id, 0x00AF, servo_angle_temp, IMODE_WRITE);
return 0;
}
diff --git a/arm/arm/main.c b/arm/arm/main.c
index b73944e8..7c0160f3 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -50,56 +50,29 @@ int main(void){
/************************************************************************/
/* TEST BELOW */
/************************************************************************/
- /**lower_arm(0);
- _delay_ms(10000);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- increment_servo(8, 20);
- _delay_ms(2000);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- increment_servo(8, -20);
- _delay_ms(2000);
+ lower_arm(1);
+ _delay_ms(5000);
+
+ arm_forward();
+ _delay_ms(300);
+ arm_forward();
+ _delay_ms(300);
+ arm_forward();
+ _delay_ms(300);
+ arm_forward();
+ _delay_ms(300);
+ arm_forward();
+ _delay_ms(300);
+
raise_arm();
- arm_rotate_default(0x00FF, IMODE_WRITE);
- _delay_ms(3000);
+ _delay_ms(4000);
disable_all_torque();
- */
/************************************************************************/
/* TEST ABOVE */
/************************************************************************/
- lower_arm(0);
- _delay_ms(8000);
- for (int i=0; i<5; i++)
- {
- increment_servo(8,50);
- _delay_ms(500);
- decrement_servo(8,30);
- _delay_ms(500);
- }
-
- raise_arm();
uint8_t b1, b2, b3,b4,b5,b6,b7,b8;
diff --git a/arm/arm/styrmodul6_2.cproj b/arm/arm/styrmodul6_2.cproj
index 22033b1b..f68aae9f 100644
--- a/arm/arm/styrmodul6_2.cproj
+++ b/arm/arm/styrmodul6_2.cproj
@@ -42,6 +42,7 @@
<com_atmel_avrdbg_tool_atmelice>
<ToolOptions>
<InterfaceProperties>
+ <IspClock>125000</IspClock>
<JtagDbgClock>200000</JtagDbgClock>
</InterfaceProperties>
<InterfaceName>JTAG</InterfaceName>
--
GitLab
From a81c72e68725e380d2db7938625ba5738732c9c3 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Thu, 5 Dec 2019 13:52:24 +0100
Subject: [PATCH 17/20] Added functionality for dropping items and made
instruction calls return error values
---
arm/arm/arm.c | 123 +++++++++++++++++++++++++++++++++-----------
arm/arm/arm.h | 14 +++--
arm/arm/main.c | 22 ++------
arm/arm/steering.c | 19 +++++--
arm/arm/steering.h | 1 +
arm/arm/styr_komm.c | 98 +++++++++++++++++------------------
arm/arm/styr_komm.h | 2 +-
7 files changed, 171 insertions(+), 108 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index a5b31321..37f5f552 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -17,7 +17,7 @@
uint16_t servo_angle[8];
-uint16_t servo_cw_limits[8] = {0,
+uint16_t servo_cw_limits[8] = {818,
0,
0,
0x01FF,//4
@@ -25,7 +25,7 @@ uint16_t servo_cw_limits[8] = {0,
0x00FF,
0,
0};
-uint16_t servo_ccw_limits[8] = {0x03FF,//1
+uint16_t servo_ccw_limits[8] = {818,//1
0x03FF,//2
0x03FF,//3
0x03FF,//4
@@ -139,10 +139,10 @@ uint8_t get_addr(char name[]){
*/
uint8_t set_led(uint8_t id, uint16_t led_value, uint8_t imode){
if(led_value != 0 && led_value != 1){
- return 1;
+ return ERROR_CONSTANT;
}
write_two_bytes(id, "LED", led_value, 1, imode);
- return 0;
+ return OK_CONSTANT;
}
/**
@@ -193,7 +193,7 @@ void enable_all_torque() {
uint8_t raise_arm(){
if(arm_lowered != 1){
- return 1; //Arm is not lowered. Do not raise
+ return ERROR_CONSTANT; //Arm is not lowered. Do not raise
}
move_to_angle(2,0x00AF,0x031F,IMODE_REG_WRITE);
move_to_angle(5,0x00AF,0x0300,IMODE_REG_WRITE);
@@ -202,8 +202,16 @@ uint8_t raise_arm(){
send_action();
_delay_ms(2000);
arm_lowered = 0;
- return 0;
+ return OK_CONSTANT;
}
+
+void return_arm(){
+
+ raise_arm();
+ arm_rotate_default(0x00FF,IMODE_WRITE);
+ disable_all_torque();
+}
+
/**
* Lowers the robot arm to a position where it should be able to pick up stuff
* Takes a direction for which side the body should rotate to first, 1 for right, 0 for left
@@ -213,7 +221,7 @@ uint8_t raise_arm(){
*/
uint8_t lower_arm(uint8_t dir){
if(arm_lowered == 1){
- return 1;//Arm already lowered
+ return ERROR_CONSTANT;//Arm already lowered
}
if(dir == 1){
@@ -225,7 +233,7 @@ uint8_t lower_arm(uint8_t dir){
servo_angle[0] = 0x00D5;
}
else{
- return 1;//Bad dir
+ return ERROR_CONSTANT;//Bad dir
}
set_torque(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
@@ -241,17 +249,64 @@ uint8_t lower_arm(uint8_t dir){
move_to_angle(4, 0x00FF, 400, IMODE_REG_WRITE);
move_to_angle(3, 0x00A0, 700, IMODE_REG_WRITE);
- move_to_angle(8,0x00FF,0x01FF,IMODE_REG_WRITE);
send_action();
_delay_ms(1000);
move_to_angle(6, 0x00FF, 818,IMODE_REG_WRITE);
- move_to_angle(8, 0x00FF, 0x01FF,IMODE_REG_WRITE);//Open claw
move_to_angle(7, 0x00FF, 0x01FF, IMODE_REG_WRITE);//Just for default value, this is never changed
send_action();
-
+ _delay_ms(5000);
arm_lowered = 1;
- return 0;
+ return OK_CONSTANT;
+}
+
+uint8_t drop_item(uint8_t nr){
+ if(nr > 3 || nr < 1){ //The robot will only be able to drop wares at 3 positions
+ return ERROR_CONSTANT;
+ }
+
+ lower_arm(1);
+ for (int i = 0; i < 5; i++)
+ {
+ arm_forward();
+ }
+
+ switch(nr){
+ case 1:
+ move_to_angle(1, 0x008F, 886, IMODE_WRITE);
+ break;
+ case 2:
+ move_to_angle(1, 0x008F, 818, IMODE_WRITE);
+ break;
+ case 3:
+ move_to_angle(1, 0x008F, 750, IMODE_WRITE);
+ break;
+ default:
+ return ERROR_CONSTANT;
+ }
+ _delay_ms(1000);
+ open_claw();
+ _delay_ms(1000);
+
+ for (int i = 0; i < 4; i++)
+ {
+ arm_backward();
+ }
+
+ _delay_ms(2000);
+ move_to_angle(1, 0x00AF, 818, IMODE_WRITE);
+
+ _delay_ms(1000);
+ return_arm();
+
+}
+
+void close_claw(){
+ move_to_angle(CLAW_ID, 0x00FF, 200, IMODE_WRITE);
+}
+
+void open_claw(){
+ move_to_angle(CLAW_ID, 0x00FF, 0x01FF, IMODE_WRITE);
}
/**
@@ -261,7 +316,7 @@ uint8_t lower_arm(uint8_t dir){
*/
uint8_t arm_forward(){
if(arm_lowered != 1){
- return 1; //Don't call if not lowered
+ return ERROR_CONSTANT; //Don't call if not lowered
}
if( (servo_angle[5]-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
@@ -282,7 +337,7 @@ uint8_t arm_forward(){
move_to_angle(4, 0x00FF, servo_angle[3], IMODE_REG_WRITE);
_delay_ms(100);
send_action();
- return 0;
+ return OK_CONSTANT;
}
/**
@@ -293,7 +348,7 @@ uint8_t arm_forward(){
*/
uint8_t arm_backward(){
if(arm_lowered != 1){
- return 1;
+ return ERROR_CONSTANT;
}
if( (servo_angle[5]+ARM_FORWARD_DELTA) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
@@ -312,9 +367,9 @@ uint8_t arm_backward(){
move_to_angle(6, 0x00FF, servo_angle[5], IMODE_REG_WRITE);
move_to_angle(4, 0x00FF, servo_angle[3], IMODE_REG_WRITE);
- _delay_ms(100);
+ _delay_ms(200);
send_action();
- return 0;
+ return OK_CONSTANT;
}
@@ -329,9 +384,9 @@ uint8_t arm_backward(){
* Returns a non-zero value on failure
*/
uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_t imode){
- if(id == ALL_SERVOS || (id == 1 && arm_lowered)){ // Please don't.
+ if(id == ALL_SERVOS){ // Please don't.
// Don't rotate the arm when arm is down
- return 1;
+ return ERROR_CONSTANT;
}
uint8_t lower_byte_pos = goal_position& 0xFF;
@@ -374,43 +429,51 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
servo_angle[id-1] = goal_position;
send_instruction(id, imode, params, 5);
}
- return 0;
+
+ return OK_CONSTANT;
+}
+
+uint8_t move_and_send_action(uint8_t id, uint16_t speed, uint16_t goal_position){
+ uint8_t return_status = move_to_angle(id, speed, goal_position, IMODE_REG_WRITE);//arg1 is id arg2 is goal_position
+ send_action();//Always send_action since we might call a pair of servos
+ return return_status;
}
-void arm_rotate_90_left(uint16_t speed,uint8_t imode){
- move_to_angle(1, speed, 0x00D5,imode);
+
+uint8_t arm_rotate_90_left(uint16_t speed,uint8_t imode){
+ return move_to_angle(1, speed, 0x00D5,imode);
}
-void arm_rotate_90_right(uint16_t speed,uint8_t imode){
- move_to_angle(1, speed, 0x0332,imode);
+uint8_t arm_rotate_90_right(uint16_t speed,uint8_t imode){
+ return move_to_angle(1, speed, 0x0332,imode);
}
uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
volatile uint16_t servo_angle_temp = servo_angle[id-1] + delta_angle;
volatile test = 1;
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
- return 1;
+ return ERROR_CONSTANT;
}
move_to_angle(id, 0x00AF, servo_angle_temp, IMODE_WRITE);
- return 0;
+ return OK_CONSTANT;
}
uint8_t decrement_servo(uint8_t id, uint16_t delta_angle) {
volatile uint16_t servo_angle_temp = servo_angle[id-1] - delta_angle;
volatile test = 1;
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
- return 1;
+ return ERROR_CONSTANT;
}
move_to_angle(id, 0x00FF, servo_angle_temp, IMODE_WRITE);
- return 0;
+ return OK_CONSTANT;
}
/**
* Returns the arm to its "resting position".
* Used after item has been grabbed and the car is about to drive.
*/
-void arm_rotate_default(uint16_t speed,uint8_t imode){
- move_to_angle(1, speed, 0x01FF,imode);
+uint8_t arm_rotate_default(uint16_t speed,uint8_t imode){
+ return move_to_angle(1, speed, 0x01FF,imode);
}
/**
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 1dbcf880..daa2c154 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -26,7 +26,7 @@
#define SERVO_4_UPPER_LIMIT 0x03FF
#define SERVO_4_LOWER_LIMIT 0x0000
-#define ARM_FORWARD_DELTA 10 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
+#define ARM_FORWARD_DELTA 20 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
@@ -36,6 +36,8 @@
#define SERVO_90_RIGHT 205 //Position for 90 degrees to the right
#define SERVO_90_LEFT 818 //Position for 90 degrees to the left
+#define ERROR_CONSTANT 254
+#define OK_CONSTANT 255
uint8_t get_addr(char name[]);
@@ -79,15 +81,19 @@ void set_max_torque(uint8_t id, uint16_t torque, uint8_t imode);
void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode);
uint8_t raise_arm();
uint8_t lower_arm(uint8_t dir);
-void arm_rotate_90_right(uint16_t speed,uint8_t imode);
-void arm_rotate_90_left(uint16_t speed,uint8_t imode);
+uint8_t arm_rotate_90_right(uint16_t speed,uint8_t imode);
+uint8_t arm_rotate_90_left(uint16_t speed,uint8_t imode);
void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
uint8_t arm_forward();
uint8_t arm_backward();
-void arm_rotate_default(uint16_t speed,uint8_t imode);
+uint8_t arm_rotate_default(uint16_t speed,uint8_t imode);
void enable_all_torque();
uint8_t increment_servo(uint8_t id, uint16_t delta_angle);
void disable_all_torque();
uint8_t decrement_servo(uint8_t id, uint16_t delta_angle);
+void close_claw();
+uint8_t drop_item(uint8_t nr);
+void open_claw();
+uint8_t move_and_send_action(uint8_t id, uint16_t speed, uint16_t goal_position);
#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 7c0160f3..8df79725 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -50,23 +50,7 @@ int main(void){
/************************************************************************/
/* TEST BELOW */
/************************************************************************/
- lower_arm(1);
- _delay_ms(5000);
-
- arm_forward();
- _delay_ms(300);
- arm_forward();
- _delay_ms(300);
- arm_forward();
- _delay_ms(300);
- arm_forward();
- _delay_ms(300);
- arm_forward();
- _delay_ms(300);
-
- raise_arm();
- _delay_ms(4000);
- disable_all_torque();
+
/************************************************************************/
/* TEST ABOVE */
@@ -80,8 +64,8 @@ int main(void){
b1 = getchar();
b2 = getchar();
b3 = getchar();
- translate_instruction(b1,b2,b3);
- putchar(255);
+ putchar(translate_instruction(b1,b2,b3));
+
}
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index 959f1ade..3abb57af 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -11,6 +11,8 @@
#define bit_set(port, bit) (port) |= (1<<(bit))
#define bit_clear(port, bit) (port) &= ~(1<<(bit))
#define bit_get(port, bit) (port) & (1<<(bit))
+#define ERROR_CONSTANT 254
+
#include <avr/io.h>
#include <util/delay.h>
@@ -92,10 +94,6 @@ void follow_line(int16_t ctrl_signal) {
abs_right_dir = (uint8_t) (abs(new_right_speed));
}
set_right_speed(abs_right_dir);
-
- putchar(default_speed);
- putchar(left_speed);
- putchar(right_speed);
}
void center_line(int16_t ctrl_signal) {
@@ -167,6 +165,7 @@ void toggle_direction(){
void set_speed(uint8_t duty_threshold){
set_left_speed(duty_threshold);
set_right_speed(duty_threshold);
+
}
void rotate_right(){
@@ -212,4 +211,16 @@ void small_forward(){
set_speed(DEFAULT_SPEED);
_delay_ms(1000);
set_speed(0);
+}
+
+uint8_t get_speed(uint8_t side){
+ if (side == 1){
+ return right_speed;
+ }
+ else if (side == 0)
+ {
+ return left_speed;
+ }
+ return ERROR_CONSTANT;
+
}
\ No newline at end of file
diff --git a/arm/arm/steering.h b/arm/arm/steering.h
index 99e53bb5..de0ca7d7 100644
--- a/arm/arm/steering.h
+++ b/arm/arm/steering.h
@@ -46,6 +46,7 @@ void drive_one_block();
void back_one_block();
void set_default_speed(uint8_t duty_threshold);
void small_forward();
+uint8_t get_speed(uint8_t side);
#endif
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index f3dd75a4..5351c0a2 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -12,6 +12,10 @@
//This is the return value returned when the pi initially asks to identify this unit.
#define STYRMODUL_ID 10
+#define INSTRUCTION_NOT_FOUND 253
+#define ERROR_CONSTANT 254
+#define OK_CONSTANT 255
+
/**
* Executes an instruction according to the given instruction number and the given arguments
*
@@ -20,124 +24,118 @@
* param arg2: second argument to instruction
*
*/
-void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
+uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
+
switch (instr) {
case 255: //Identify
- putchar(STYRMODUL_ID);
+ return STYRMODUL_ID;
break;
case 0: //Forward
set_direction(FORWARD);
set_speed(arg1);
- break;
+ return OK_CONSTANT;
case 1: //Backward
set_direction(BACK);
set_speed(arg1);
- break;
+ return OK_CONSTANT;
case 2://Turn left
set_direction(FORWARD);
set_right_speed(arg1);
set_left_speed(arg1*TURN_CONSTANT);
- break;
+ return OK_CONSTANT;
case 3: //Turn right
set_direction(FORWARD);
set_right_speed(arg1*TURN_CONSTANT);
set_left_speed(arg1);
- break;
+ return OK_CONSTANT;
case 4: //Rotate left
set_speed(arg1);
rotate_left();
- break;
+ return OK_CONSTANT;
case 5: //Rotate right
set_speed(arg1);
rotate_right();
- break;
+ return OK_CONSTANT;
case 6://STOP
set_speed(0);
- break;
+ return OK_CONSTANT;
case 7://Set driving speed
set_speed(arg1);
- break;
+ return OK_CONSTANT;
case 8: //move to angle
- move_to_angle(arg1, 0x00FF, arg2, IMODE_REG_WRITE);//arg1 is id arg2 is goal_position
- send_action();//Always send_action since we might call a pair of servos
- break;
+ return move_and_send_action(arg1, 0x00FF, arg2);
case 9: //stop servo
set_torque(arg1,0,IMODE_WRITE);
- break;
+ return OK_CONSTANT;
case 10: //get angle
//This cant be used for servo 1 - 6 due to technical difficulties, and has been decided to not be implemented
- break;
+ return ERROR_CONSTANT;
case 11: //get temperature
//This cant be used for servo 1 - 6 due to technical difficulties, and has been decided to not be implemented
- break;
+ return ERROR_CONSTANT;
case 12: //get voltage
//This cant be used for servo 1 - 6 due to technical difficulties, and has been decided to not be implemented
- break;
+ return ERROR_CONSTANT;
case 13: //set servo speed
set_moving_speed(arg1, arg2, IMODE_WRITE);
- break;
+ return OK_CONSTANT;
case 15: //open claw
- move_to_angle(CLAW_ID, arg1, 512, IMODE_WRITE);
- break;
+ open_claw();
+ return OK_CONSTANT;
case 16: //close claw
- move_to_angle(CLAW_ID, arg1, 0, IMODE_WRITE);//Right now the claw will close fully, we probably
- break; //want it to close halway or something
+ close_claw();//TODO set to an angle that fits the item we will pickup
+ return OK_CONSTANT;
case 17: // rotate arm left
- arm_rotate_90_left(arg1,IMODE_WRITE);
- break;
+ return arm_rotate_90_left(arg1,IMODE_WRITE);
case 18: //rotate arm right
- arm_rotate_90_right(arg1,IMODE_WRITE);
- break;
+ return arm_rotate_90_right(arg1,IMODE_WRITE);
case 19: //arm down
- lower_arm(arg1);
- break;
+ return lower_arm(arg1);
case 20://arm up
- raise_arm();
- break;
+ return raise_arm();
case 21: //arm forward
- arm_forward();
- break;
+ return arm_forward();
case 22: //arm backward
- arm_backward();
- break;
+ return arm_backward();
case 50: //rotate 90 left
rotate_90_left();
- break;
+ return OK_CONSTANT;
case 51: //rotate 90 right
- rotate_right();
- break;
+ rotate_90_right();
+ return OK_CONSTANT;
case 52: //Small forward
small_forward();
- break;
+ return OK_CONSTANT;
case 53: //Disable all torque
disable_all_torque();
- break;
+ return OK_CONSTANT;
case 55: //Enable all torque
enable_all_torque();
- break;
+ return OK_CONSTANT;
case 54: //Rotate default
- arm_rotate_default(arg1,IMODE_WRITE);
- break;
+ return arm_rotate_default(arg1,IMODE_WRITE);
case 56: //Increment servo
- increment_servo(arg1, INCREMENT_CONSTANT);
- break;
+ return increment_servo(arg1, INCREMENT_CONSTANT);
case 57: //Decrement servo
- decrement_servo(arg1, INCREMENT_CONSTANT);
- break;
+ return decrement_servo(arg1, INCREMENT_CONSTANT);
+ case 58:
+ return drop_item(arg1);
+ case 59:
+ return get_speed(arg1);
case 100:
// Rotate to center line
set_speed(120);
center_line(update_ctrl_signal((((uint16_t)(arg1)) << 8) + arg2));
- break;
+ return OK_CONSTANT;
case 101:
// Read line data
follow_line(update_ctrl_signal((((uint16_t)(arg1)) << 8) + arg2));
- break;
+ return OK_CONSTANT;
case 102:
// Set default speed
set_default_speed(arg1);
- break;
+ return OK_CONSTANT;
default:
- break;
+ return ERROR_CONSTANT;
}
}
\ No newline at end of file
diff --git a/arm/arm/styr_komm.h b/arm/arm/styr_komm.h
index 72d968fb..fab5cd88 100644
--- a/arm/arm/styr_komm.h
+++ b/arm/arm/styr_komm.h
@@ -10,7 +10,7 @@
#define STYR_KOMM_H_
-void translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2);
+uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2);
#endif /* STYR_KOMM_H_ */
\ No newline at end of file
--
GitLab
From 1a4bbb17a7151c7dff9c8bae094cce6291501fa2 Mon Sep 17 00:00:00 2001
From: Nils Johansson <niljo502@student.liu.se>
Date: Mon, 9 Dec 2019 16:25:30 +0100
Subject: [PATCH 18/20] Commmented and cleaned up code
---
arm/arm/arm.c | 75 +++++++++++++++++++++++++++++----------------
arm/arm/arm.h | 17 +++-------
arm/arm/main.c | 17 +++-------
arm/arm/serial.c | 3 +-
arm/arm/steering.c | 55 ++++++++++++++++++---------------
arm/arm/steering.h | 5 +--
arm/arm/styr_komm.c | 11 ++++---
7 files changed, 99 insertions(+), 84 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 37f5f552..d7bdb24f 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -17,15 +17,17 @@
uint16_t servo_angle[8];
-uint16_t servo_cw_limits[8] = {818,
- 0,
- 0,
+//Clockwise and counterclockwise limits for servos 1-8
+uint16_t servo_cw_limits[8] = {818, //1
+ 0, //2
+ 0, //3
0x01FF,//4
0x01FF,//5
- 0x00FF,
- 0,
- 0};
-uint16_t servo_ccw_limits[8] = {818,//1
+ 0x00FF,//6
+ 0, //7
+ 0}; //8
+
+uint16_t servo_ccw_limits[8] = {818, //1
0x03FF,//2
0x03FF,//3
0x03FF,//4
@@ -191,6 +193,7 @@ void enable_all_torque() {
}
}
+//Raises the arm if it has been lowered to pickup an item
uint8_t raise_arm(){
if(arm_lowered != 1){
return ERROR_CONSTANT; //Arm is not lowered. Do not raise
@@ -205,8 +208,8 @@ uint8_t raise_arm(){
return OK_CONSTANT;
}
+//Raises the arm and disables torque so the arm rests in an idle state
void return_arm(){
-
raise_arm();
arm_rotate_default(0x00FF,IMODE_WRITE);
disable_all_torque();
@@ -260,10 +263,20 @@ uint8_t lower_arm(uint8_t dir){
return OK_CONSTANT;
}
+/**
+ * Drops an item picked up by the claw at 1 of 3 predefined positions
+ * takes a uint8_t nr that specifies which position to drop the item at
+ * meant to be used when the arm has been raised
+ *
+ * Returns the arm to an idle position after the item has been dropped
+ */
uint8_t drop_item(uint8_t nr){
if(nr > 3 || nr < 1){ //The robot will only be able to drop wares at 3 positions
return ERROR_CONSTANT;
}
+ if(arm_lowered == 1){//If arm is lowered, return
+ return ERROR_CONSTANT;
+ }
lower_arm(1);
for (int i = 0; i < 5; i++)
@@ -319,15 +332,15 @@ uint8_t arm_forward(){
return ERROR_CONSTANT; //Don't call if not lowered
}
- if( (servo_angle[5]-ARM_FORWARD_DELTA) <= SERVO_6_LOWER_LIMIT){//If overflow or limit is reached
- servo_angle[5] = SERVO_6_LOWER_LIMIT;
+ if( (servo_angle[5]-ARM_FORWARD_DELTA) <= servo_cw_limits[5]){//If limit is reached
+ servo_angle[5] = servo_cw_limits[5];
}
else{
servo_angle[5] = servo_angle[5] - ARM_FORWARD_DELTA;
}
- if( (servo_angle[3] + ARM_FORWARD_DELTA) >= SERVO_4_UPPER_LIMIT){//If overflow or limit is reached
- servo_angle[3] = SERVO_4_UPPER_LIMIT;
+ if( (servo_angle[3] + ARM_FORWARD_DELTA) >= servo_ccw_limits[3]){//If limit is reached
+ servo_angle[3] = servo_ccw_limits[3];
}
else{
servo_angle[3] = servo_angle[3]+ ARM_FORWARD_DELTA;
@@ -351,15 +364,15 @@ uint8_t arm_backward(){
return ERROR_CONSTANT;
}
- if( (servo_angle[5]+ARM_FORWARD_DELTA) >= SERVO_6_UPPER_LIMIT){//If overflow or limit is reached
- servo_angle[5] = SERVO_6_UPPER_LIMIT;
+ if( (servo_angle[5]+ARM_FORWARD_DELTA) >= servo_ccw_limits[5]){//If limit is reached
+ servo_angle[5] = servo_ccw_limits[5];
}
else{
servo_angle[5] = servo_angle[5] + ARM_FORWARD_DELTA;
}
- if( servo_angle[3]-ARM_FORWARD_DELTA <= SERVO_4_LOWER_LIMIT){//If overflow or limit is reached
- servo_angle[3] = SERVO_4_LOWER_LIMIT;
+ if( servo_angle[3]-ARM_FORWARD_DELTA <= servo_cw_limits[3]){//If limit is reached
+ servo_angle[3] = servo_cw_limits[3];
}
else{
servo_angle[3] = servo_angle[3] - ARM_FORWARD_DELTA;
@@ -389,7 +402,7 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
return ERROR_CONSTANT;
}
- uint8_t lower_byte_pos = goal_position& 0xFF;
+ uint8_t lower_byte_pos = goal_position& 0xFF;//Split up arguments into bytes
uint8_t higher_byte_pos = goal_position >> 8;
uint8_t lower_byte_speed = speed & 0xFF;
@@ -399,8 +412,9 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
if (id == 2 || id == 3 || id == 4 || id == 5){//Servo-pairs (2 and 3) and (4 and 5) should always move together with "opposite" goal_position
servo_angle[id-1] = goal_position; //Update current servo angle for given servo id
- send_instruction(id, IMODE_REG_WRITE, params, 5);
+ send_instruction(id, IMODE_REG_WRITE, params, 5);//move first servo
_delay_ms(200);
+
uint16_t goal_position_2 = MAX_GOAL_POSITION - goal_position;
uint8_t lower_byte_pos_2 = goal_position_2 & 0xFF;
uint8_t higher_byte_pos_2 = goal_position_2 >> 8;
@@ -433,24 +447,33 @@ uint8_t move_to_angle(uint8_t id, uint16_t speed, uint16_t goal_position, uint8_
return OK_CONSTANT;
}
+// Function that calls move_to_angle with IMODE_REG_WRITE and sends an send_action directly after
+// returns the returned status by move_to_angle
uint8_t move_and_send_action(uint8_t id, uint16_t speed, uint16_t goal_position){
uint8_t return_status = move_to_angle(id, speed, goal_position, IMODE_REG_WRITE);//arg1 is id arg2 is goal_position
send_action();//Always send_action since we might call a pair of servos
return return_status;
}
-
+ /*
+ * Rotates servo 1 to a position where the arm, in its idle state,
+ * is 90 degrees to the left
+ */
uint8_t arm_rotate_90_left(uint16_t speed,uint8_t imode){
return move_to_angle(1, speed, 0x00D5,imode);
}
+ /*
+ * Rotates servo 1 to a position where the arm, in its idle state,
+ * is 90 degrees to the right
+ */
uint8_t arm_rotate_90_right(uint16_t speed,uint8_t imode){
return move_to_angle(1, speed, 0x0332,imode);
}
+//Increments the position of the servo with the given id by the given delta_angle
uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
- volatile uint16_t servo_angle_temp = servo_angle[id-1] + delta_angle;
- volatile test = 1;
+ uint16_t servo_angle_temp = servo_angle[id-1] + delta_angle;
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
return ERROR_CONSTANT;
}
@@ -458,9 +481,9 @@ uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
return OK_CONSTANT;
}
+//Decrements the position of the servo with the given id by the given delta_angle
uint8_t decrement_servo(uint8_t id, uint16_t delta_angle) {
- volatile uint16_t servo_angle_temp = servo_angle[id-1] - delta_angle;
- volatile test = 1;
+ uint16_t servo_angle_temp = servo_angle[id-1] - delta_angle;
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
return ERROR_CONSTANT;
}
@@ -488,7 +511,7 @@ void set_ccw_limit(uint8_t id, uint16_t limit, uint8_t imode){
void set_cw_limit(uint8_t id, uint16_t limit, uint8_t imode){
write_two_bytes(id, "CW Angle Limit", limit, 2, imode);
}
-
+//Set return delay of return package
void set_return_delay(uint8_t id, uint16_t delay, uint8_t imode){
write_two_bytes(id, "Return Delay Time", delay,1,imode);
}
@@ -512,7 +535,6 @@ void write_two_bytes(uint8_t id, char *name, uint16_t arg, uint8_t param_len, ui
/**
- *
* param id: servo id
* param imode: instruction mode
* param params: array of parameters
@@ -540,14 +562,13 @@ void send_instruction(uint8_t id, uint8_t imode, uint8_t *params, uint8_t param_
uart_1_putchar(checksum);
}
-
+//Ping a servo with a given id
void ping(uint8_t id) {
uart_1_putchar(255); //H1
uart_1_putchar(255); //H2
uart_1_putchar(id); //ID
uart_1_putchar(2); //LEN
uart_1_putchar(1); //INST
- //uart_1_putchar(0xFB);
uart_1_putchar(0xFF - (id + 2 + 1)); //Checksum calculation
}
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index daa2c154..facadc0f 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -8,34 +8,27 @@
#ifndef ARM_H_
#define ARM_H_
-#define INSTRUCTION_SET_LENGTH 49
+#define INSTRUCTION_SET_LENGTH 49 //Max instruction address for arm servos
#define ALL_SERVOS 0xFE //ID for calling all servos
#define CLAW_ID 8 // TODO check id of claw
+//Instruction modes
#define IMODE_READ 2
#define IMODE_WRITE 3
#define IMODE_REG_WRITE 4
#define IMODE_ACTION 5
+//Max and min position of the AX12 Dynamixel servos
#define MAX_GOAL_POSITION 0x03FF
#define MIN_GOAL_POSITION 0x0000
-#define SERVO_6_UPPER_LIMIT 0x0330
-#define SERVO_6_LOWER_LIMIT 0x00FF
-
-#define SERVO_4_UPPER_LIMIT 0x03FF
-#define SERVO_4_LOWER_LIMIT 0x0000
-
#define ARM_FORWARD_DELTA 20 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
-#define INCREMENT_CONSTANT 10 //Check this
-
-//TODO test these positions
-#define SERVO_90_RIGHT 205 //Position for 90 degrees to the right
-#define SERVO_90_LEFT 818 //Position for 90 degrees to the left
+#define INCREMENT_CONSTANT 80 //Value used to increment and decrement position of servos
+//Return value constants
#define ERROR_CONSTANT 254
#define OK_CONSTANT 255
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 8df79725..95431976 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -5,19 +5,13 @@
* Author: niljo502
*/
-/*
-TODO:
-1. There might be something wrong with the write_to_servo() and/or send_instruction(). FIXME!
-2. Follow the code skeleton in dev/komm in AVR/ECU_unit/src/main.c for correct communicatin with raspi.
-3. Write/structure up the arm-interface for easy movement.
-*/
-
+//Help-functions for accessing and modifying bytes
#define bit_set(port, bit) (port) |= (1<<(bit))
#define bit_clear(port, bit) (port) &= ~(1<<(bit))
#define bit_get(port, bit) (port) & (1<<(bit))
#define F_CPU 16000000L
-#define BAUD 19200 // for serial 1M
+#define BAUD 19200
#include <avr/io.h>
@@ -28,9 +22,9 @@ TODO:
#include "serial.h"
#include "arm.h"
-
#include "styr_komm.h"
+//Setup communication with RaspberryPi
FILE uart_output = FDEV_SETUP_STREAM(uart_0_putchar, NULL, _FDEV_SETUP_WRITE);
FILE uart_input = FDEV_SETUP_STREAM(NULL, uart_0_getchar, _FDEV_SETUP_READ);
@@ -38,8 +32,6 @@ bool cold();
void uart_init();
void uart_arm_init();
-#define BUFFER_SIZE 10
-
int main(void){
//cold start.
@@ -73,12 +65,13 @@ int main(void){
}
-/* HW init. Return true on success, false if some problem occurred. .*/
+/* init function. Return true on success, false if some problem occurred. */
bool cold(){
// Initializes serial communication.
uart_init();
uart_arm_init();
+ //Initialize steering and arm
pwm_init();
servo_init();
diff --git a/arm/arm/serial.c b/arm/arm/serial.c
index 52ef7ee5..70f05e47 100644
--- a/arm/arm/serial.c
+++ b/arm/arm/serial.c
@@ -12,12 +12,13 @@
#include "serial.h"
-
+//Write one byte to UART stream
void uart_0_putchar(char c, FILE *stream){
loop_until_bit_is_set(UCSR0A, UDRE0);
UDR0 = c;
}
+//Read one byte from UART stream
char uart_0_getchar(FILE *stream){
loop_until_bit_is_set(UCSR0A, RXC0);
return UDR0;
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index 3abb57af..a9a5bb0b 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -13,11 +13,9 @@
#define bit_get(port, bit) (port) & (1<<(bit))
#define ERROR_CONSTANT 254
-
#include <avr/io.h>
#include <util/delay.h>
#include <stdbool.h>
-
#include "steering.h"
uint8_t right_speed = 0;//Current speed of the right wheel pair
@@ -33,6 +31,7 @@ enum Direction DEFAULT_DIR = FORWARD;
* The range of the error values are: -32768 to 32767
* -32768 < left < 0 < right < 32767
**/
+
int16_t prev_error;
/**
@@ -121,17 +120,11 @@ void set_left_speed(uint8_t duty_threshold){
left_speed = duty_threshold;
}
-void toggle_right_direction(){
- PORTB ^= _BV(0);//toggle bit
- right_dir = !right_dir;
-}
-
-void toggle_left_direction(){
- PORTB ^= _BV(1);//toggle bit
- left_dir = !left_dir;
-}
-
+//Set direction of left wheel pair
void set_left_direction(enum Direction dir){
+ if(dir != 0 && dir != 1){
+ return;
+ }
if (dir == FORWARD){
bit_set(PORTB, 0);
}
@@ -139,9 +132,11 @@ void set_left_direction(enum Direction dir){
bit_clear(PORTB, 0);
}
}
-
+//Set direction of right wheel pair
void set_right_direction(enum Direction dir){
-
+ if(dir != 0 && dir != 1){
+ return;
+ }
if (dir == FORWARD){
bit_clear(PORTB, 1);
}
@@ -150,34 +145,37 @@ void set_right_direction(enum Direction dir){
}
}
+//Sets the direction of both wheel pairs to the given dir
void set_direction(enum Direction dir){
set_right_direction(dir);
set_left_direction(dir);
}
-void toggle_direction(){
- toggle_left_direction();
- toggle_right_direction();
-}
-
-//Valid range for duty_threshhold is 0-255
-//Use higher threshold for higher speed
+/**
+ * Takes a compare value duty_threshold whisch is used to generate the PWM-signal
+ * Valid range for duty_threshhold is 0-255
+ * Use higher threshold for higher speed
+ */
void set_speed(uint8_t duty_threshold){
set_left_speed(duty_threshold);
set_right_speed(duty_threshold);
}
+//Sets the direction of the wheels to oposite directions so the robot will rotate to the left
void rotate_right(){
set_left_direction(FORWARD);
set_right_direction(BACK);
}
+//Sets the direction of the wheels to oposite directions so the robot will rotate to the right
void rotate_left(){
set_left_direction(BACK);
set_right_direction(FORWARD);
}
+
+//Rotates the robot 90 degrees to the left
void rotate_90_left(){//TODO test and adjust so it actually rotates 90 degrees
set_speed(DEFAULT_ROTATE_SPEED);
rotate_left();
@@ -186,6 +184,7 @@ void rotate_90_left(){//TODO test and adjust so it actually rotates 90 degrees
set_speed(DEFAULT_SPEED);
}
+//Rotates the robot 90 degrees to the right
void rotate_90_right(){//TODO test and adjust so it actually rotates 90 degrees
set_speed(DEFAULT_ROTATE_SPEED);
rotate_right();
@@ -194,11 +193,15 @@ void rotate_90_right(){//TODO test and adjust so it actually rotates 90 degrees
set_speed(DEFAULT_SPEED);
}
+//Drives the robot on block forward
+//One block is about 1 meter
void drive_one_block(){
set_speed(DEFAULT_SPEED);
_delay_ms(DRIVE_ONE_BLOCK_DELAY);
}
+//Function that drives the robot one "block" backward
+//One block is about 1 meter
void back_one_block(){
set_direction(BACK);
set_speed(DEFAULT_SPEED);
@@ -206,19 +209,23 @@ void back_one_block(){
set_direction(FORWARD);
}
+//Function that drives the robot a small distance forward
void small_forward(){
set_direction(FORWARD);
set_speed(DEFAULT_SPEED);
_delay_ms(1000);
set_speed(0);
}
-
+/**
+ * Returns the the compare value used for PWM-signal generation for
+ * The right wheel pair if given side is 1
+ * The left wheel pair if given side is 0
+ */
uint8_t get_speed(uint8_t side){
if (side == 1){
return right_speed;
}
- else if (side == 0)
- {
+ else if (side == 0){
return left_speed;
}
return ERROR_CONSTANT;
diff --git a/arm/arm/steering.h b/arm/arm/steering.h
index de0ca7d7..c277c474 100644
--- a/arm/arm/steering.h
+++ b/arm/arm/steering.h
@@ -14,7 +14,7 @@ enum Direction{BACK, FORWARD};
#define DEFAULT_SPEED 100
#define DEFAULT_ROTATE_SPEED 100
-#define TURN_CONSTANT 0.5
+#define TURN_CONSTANT 0.2
#define ROTATE_90_DELAY 2000
#define DRIVE_ONE_BLOCK_DELAY 3400
#define TEST_SPEED 80
@@ -31,12 +31,9 @@ void center_line(int16_t ctrl_signal);
void set_right_speed(uint8_t duty_threshold);
void set_left_speed(uint8_t duty_threshold);
-void toggle_right_direction();
-void toggle_left_direction();
void set_left_direction(enum Direction dir);
void set_right_direction(enum Direction dir);
void set_direction(enum Direction dir);
-void toggle_direction();
void set_speed(uint8_t duty_threshold);
void rotate_right();
void rotate_left();
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 5351c0a2..12b0d997 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -12,9 +12,10 @@
//This is the return value returned when the pi initially asks to identify this unit.
#define STYRMODUL_ID 10
-#define INSTRUCTION_NOT_FOUND 253
+//Values returned to the PI
#define ERROR_CONSTANT 254
#define OK_CONSTANT 255
+#define SPEED_CONSTANT 250
/**
* Executes an instruction according to the given instruction number and the given arguments
@@ -92,7 +93,7 @@ uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
case 19: //arm down
return lower_arm(arg1);
case 20://arm up
- return raise_arm();
+ return return_arm();
case 21: //arm forward
return arm_forward();
case 22: //arm backward
@@ -120,8 +121,10 @@ uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
return decrement_servo(arg1, INCREMENT_CONSTANT);
case 58:
return drop_item(arg1);
- case 59:
- return get_speed(arg1);
+ case 59://Returns both speeds of the wheel pairs. First the left speed then the right.
+ putchar(SPEED_CONSTANT);
+ putchar(get_speed(0));
+ return(get_speed(1));
case 100:
// Rotate to center line
set_speed(120);
--
GitLab
From 6cc383c19ef934fe1bb1c1e8e5dc565f3cef7672 Mon Sep 17 00:00:00 2001
From: Jonatan Jonsson <jonjo049@student.liu.se>
Date: Wed, 11 Dec 2019 11:33:28 +0100
Subject: [PATCH 19/20] fixed raise arm, maybe
---
arm/arm/arm.c | 91 ++++++++++++++++++++++++++++++++++++---------
arm/arm/arm.h | 11 ++++--
arm/arm/main.c | 2 +-
arm/arm/styr_komm.c | 12 ++++--
4 files changed, 90 insertions(+), 26 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index d7bdb24f..64433897 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -18,16 +18,16 @@ uint16_t servo_angle[8];
//Clockwise and counterclockwise limits for servos 1-8
-uint16_t servo_cw_limits[8] = {818, //1
- 0, //2
- 0, //3
- 0x01FF,//4
- 0x01FF,//5
+uint16_t servo_cw_limits[8] = {778, //1
+ 20, //2 was 0
+ 20, //3 was 0
+ 0x0020,//4 was 01FF
+ 0x0020,//5 was 01FF
0x00FF,//6
0, //7
0}; //8
-uint16_t servo_ccw_limits[8] = {818, //1
+uint16_t servo_ccw_limits[8] = {858, //1
0x03FF,//2
0x03FF,//3
0x03FF,//4
@@ -37,6 +37,8 @@ uint16_t servo_ccw_limits[8] = {818, //1
0x03FF};
uint8_t arm_lowered = 0; //1 if instruction arm_forward should be callable 0 if not
+uint8_t arm_lowered_right = 0;
+uint8_t arm_lowered_left = 0;
//Array of instruction names. The index of the instruction string is the address of the instruction.
const char INSTRUCTIONS [INSTRUCTION_SET_LENGTH][32] = {
@@ -194,7 +196,7 @@ void enable_all_torque() {
}
//Raises the arm if it has been lowered to pickup an item
-uint8_t raise_arm(){
+uint8_t retract_arm(){
if(arm_lowered != 1){
return ERROR_CONSTANT; //Arm is not lowered. Do not raise
}
@@ -205,12 +207,14 @@ uint8_t raise_arm(){
send_action();
_delay_ms(2000);
arm_lowered = 0;
+ arm_lowered_right = 0;
+ arm_lowered_left = 0;
return OK_CONSTANT;
}
//Raises the arm and disables torque so the arm rests in an idle state
void return_arm(){
- raise_arm();
+ retract_arm();
arm_rotate_default(0x00FF,IMODE_WRITE);
disable_all_torque();
}
@@ -222,24 +226,27 @@ void return_arm(){
*
* Returns a non-zero value on failure
*/
-uint8_t lower_arm(uint8_t dir){
+uint8_t extend_arm(uint8_t dir){
if(arm_lowered == 1){
return ERROR_CONSTANT;//Arm already lowered
}
if(dir == 1){
- arm_rotate_90_right(0x00FF, IMODE_WRITE);
+ arm_rotate_90_CCW(0x00FF, IMODE_WRITE);
servo_angle[0] = 0x0332;
+ arm_lowered_left = 1;
+
}
else if(dir == 0){
- arm_rotate_90_left(0x00FF,IMODE_WRITE);
+ arm_rotate_90_CW(0x00FF,IMODE_WRITE);
servo_angle[0] = 0x00D5;
+ arm_lowered_right = 1;
}
else{
return ERROR_CONSTANT;//Bad dir
}
- set_torque(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when raise_arm is called
+ set_torque(2,1,IMODE_WRITE);//Since we disable the torque on servo 2 and 3 when retract_arm is called
set_torque(3,1,IMODE_WRITE);//we make sure that they are enabled here
_delay_ms(200);
@@ -278,7 +285,7 @@ uint8_t drop_item(uint8_t nr){
return ERROR_CONSTANT;
}
- lower_arm(1);
+ extend_arm(1);
for (int i = 0; i < 5; i++)
{
arm_forward();
@@ -385,6 +392,53 @@ uint8_t arm_backward(){
return OK_CONSTANT;
}
+uint8_t lower_arm() {
+ if (arm_lowered != 1) {
+ return ERROR_CONSTANT;
+ }
+
+ if (servo_angle[3]+ARM_FORWARD_DELTA >= servo_ccw_limits[3]){//If limit is reached
+ servo_angle[3] = servo_ccw_limits[3];
+ } else {
+ servo_angle[3] = servo_angle[3] + ARM_FORWARD_DELTA;
+ }
+
+ if (servo_angle[2]+ARM_FORWARD_DELTA >= servo_ccw_limits[2]){//If limit is reached
+ servo_angle[2] = servo_ccw_limits[2];
+ } else {
+ servo_angle[2] = servo_angle[2] + ARM_FORWARD_DELTA;
+ }
+
+ move_to_angle(4, 0x00FF, servo_angle[3], IMODE_REG_WRITE);
+ move_to_angle(3, 0x00FF, servo_angle[2], IMODE_REG_WRITE);
+ _delay_ms(200);
+ send_action();
+ return OK_CONSTANT;
+}
+
+uint8_t raise_arm() {
+ if (arm_lowered != 1) {
+ return ERROR_CONSTANT;
+ }
+
+ if (servo_angle[3]-ARM_FORWARD_DELTA <= servo_cw_limits[3]){//If limit is reached
+ servo_angle[3] = servo_cw_limits[3];
+ } else {
+ servo_angle[3] = servo_angle[3] - ARM_FORWARD_DELTA;
+ }
+
+ if (servo_angle[2]-ARM_FORWARD_DELTA <= servo_cw_limits[2]){//If limit is reached
+ servo_angle[2] = servo_cw_limits[2];
+ } else {
+ servo_angle[2] = servo_angle[2] - ARM_FORWARD_DELTA;
+ }
+
+ move_to_angle(4, 0x00FF, servo_angle[3], IMODE_REG_WRITE);
+ move_to_angle(3, 0x00FF, servo_angle[2], IMODE_REG_WRITE);
+ _delay_ms(200);
+ send_action();
+ return OK_CONSTANT;
+}
/**
* Moves the servo with the given id to a given goal position with the given speed
@@ -457,23 +511,26 @@ uint8_t move_and_send_action(uint8_t id, uint16_t speed, uint16_t goal_position)
/*
* Rotates servo 1 to a position where the arm, in its idle state,
- * is 90 degrees to the left
+ * The arm can be lowered on the right side.
*/
-uint8_t arm_rotate_90_left(uint16_t speed,uint8_t imode){
+uint8_t arm_rotate_90_CW(uint16_t speed,uint8_t imode){
return move_to_angle(1, speed, 0x00D5,imode);
}
/*
* Rotates servo 1 to a position where the arm, in its idle state,
- * is 90 degrees to the right
+ * The arm can be lowered on the left side.
*/
-uint8_t arm_rotate_90_right(uint16_t speed,uint8_t imode){
+uint8_t arm_rotate_90_CCW(uint16_t speed,uint8_t imode){
return move_to_angle(1, speed, 0x0332,imode);
}
//Increments the position of the servo with the given id by the given delta_angle
uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
uint16_t servo_angle_temp = servo_angle[id-1] + delta_angle;
+
+ //TODO: fix so that the limits for id 1 are inverted for the left side when incrementing!!!!!!!!!!!!!
+
if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
return ERROR_CONSTANT;
}
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index facadc0f..495e17ea 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -23,6 +23,7 @@
#define MIN_GOAL_POSITION 0x0000
#define ARM_FORWARD_DELTA 20 // How much servo 4 and 6 should be moved forward/backward when arm_forward()/backward is caled
+#define ARM_RAISE_DELTA 10
#define REST_POSITION_SERVO_1 0x01FF //Middle position of servo
@@ -72,10 +73,10 @@ void send_instruction(uint8_t id, uint8_t imode, uint8_t *params, uint8_t param_
void set_return_delay(uint8_t id, uint16_t delay, uint8_t imode);
void set_max_torque(uint8_t id, uint16_t torque, uint8_t imode);
void set_baud_rate(uint8_t id, uint16_t baud_rate, uint8_t imode);
-uint8_t raise_arm();
-uint8_t lower_arm(uint8_t dir);
-uint8_t arm_rotate_90_right(uint16_t speed,uint8_t imode);
-uint8_t arm_rotate_90_left(uint16_t speed,uint8_t imode);
+uint8_t retract_arm();
+uint8_t extend_arm(uint8_t dir);
+uint8_t arm_rotate_90_CW(uint16_t speed,uint8_t imode);
+uint8_t arm_rotate_90_CCW(uint16_t speed,uint8_t imode);
void torque_enable(uint8_t id, uint16_t enable, uint8_t imode);
uint8_t arm_forward();
uint8_t arm_backward();
@@ -88,5 +89,7 @@ void close_claw();
uint8_t drop_item(uint8_t nr);
void open_claw();
uint8_t move_and_send_action(uint8_t id, uint16_t speed, uint16_t goal_position);
+uint8_t raise_arm();
+uint8_t lower_arm();
#endif // ARM_H_
\ No newline at end of file
diff --git a/arm/arm/main.c b/arm/arm/main.c
index 95431976..a448ef3b 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -43,7 +43,7 @@ int main(void){
/* TEST BELOW */
/************************************************************************/
-
+
/************************************************************************/
/* TEST ABOVE */
/************************************************************************/
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index 12b0d997..df06f9f0 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -87,17 +87,21 @@ uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
close_claw();//TODO set to an angle that fits the item we will pickup
return OK_CONSTANT;
case 17: // rotate arm left
- return arm_rotate_90_left(arg1,IMODE_WRITE);
+ return arm_rotate_90_CW(arg1,IMODE_WRITE);
case 18: //rotate arm right
- return arm_rotate_90_right(arg1,IMODE_WRITE);
+ return arm_rotate_90_CCW(arg1,IMODE_WRITE);
case 19: //arm down
- return lower_arm(arg1);
- case 20://arm up
+ return extend_arm(arg1);
+ case 20://return, rotate and torque
return return_arm();
case 21: //arm forward
return arm_forward();
case 22: //arm backward
return arm_backward();
+ case 23: //raise arm
+ return raise_arm();
+ case 24: //lower arm
+ return lower_arm();
case 50: //rotate 90 left
rotate_90_left();
return OK_CONSTANT;
--
GitLab
From bf7b93b5868302c001c3c71b61918cb38fa9f6bf Mon Sep 17 00:00:00 2001
From: jonjo049 <jonjo049@student.liu.se>
Date: Thu, 12 Dec 2019 15:20:53 +0100
Subject: [PATCH 20/20] Added header to documents
---
arm/arm/arm.c | 27 ++++---
arm/arm/arm.h | 10 +--
arm/arm/main.c | 10 +--
arm/arm/steering.c | 10 +--
arm/arm/steering.h | 10 +--
arm/arm/styr_komm.c | 14 ++--
arm/arm/styr_komm.h | 10 +--
arm/arm/styrmodul.cproj | 162 ++++++++++++++++++++--------------------
8 files changed, 130 insertions(+), 123 deletions(-)
diff --git a/arm/arm/arm.c b/arm/arm/arm.c
index 64433897..67e34fb3 100644
--- a/arm/arm/arm.c
+++ b/arm/arm/arm.c
@@ -1,9 +1,10 @@
/*
- * avr_to_arm_uart.c
- *
- * Created: 2019-11-15 14:28:20
- * Author: niljo502
- */
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
+*/
+
#define F_CPU 16000000L
#include <avr/io.h>
@@ -18,7 +19,7 @@ uint16_t servo_angle[8];
//Clockwise and counterclockwise limits for servos 1-8
-uint16_t servo_cw_limits[8] = {778, //1
+uint16_t servo_cw_limits[8] = {750, //1
20, //2 was 0
20, //3 was 0
0x0020,//4 was 01FF
@@ -27,12 +28,12 @@ uint16_t servo_cw_limits[8] = {778, //1
0, //7
0}; //8
-uint16_t servo_ccw_limits[8] = {858, //1
+uint16_t servo_ccw_limits[8] = {880, //1
0x03FF,//2
0x03FF,//3
0x03FF,//4
0x03FF,//5
- 0x0330,//6
+ 818,//6
0x03FF,//7
0x03FF};
@@ -529,11 +530,15 @@ uint8_t arm_rotate_90_CCW(uint16_t speed,uint8_t imode){
uint8_t increment_servo(uint8_t id, uint16_t delta_angle) {
uint16_t servo_angle_temp = servo_angle[id-1] + delta_angle;
- //TODO: fix so that the limits for id 1 are inverted for the left side when incrementing!!!!!!!!!!!!!
-
- if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
+ /*
+ if (id = 1 && arm_lowered_right == 1 && servo_angle_temp < (MAX_GOAL_POSITION - servo_ccw_limits[0]) || servo_angle_temp > (MAX_GOAL_POSITION - servo_cw_limits[0])) {
+ return ERROR_CONSTANT;
+ } else if (servo_angle_temp < servo_cw_limits[id-1] || servo_angle_temp > servo_ccw_limits[id-1]) {
return ERROR_CONSTANT;
}
+ */
+
+ servo_angle[id-1] = servo_angle_temp;
move_to_angle(id, 0x00AF, servo_angle_temp, IMODE_WRITE);
return OK_CONSTANT;
}
diff --git a/arm/arm/arm.h b/arm/arm/arm.h
index 495e17ea..0ca4edf0 100644
--- a/arm/arm/arm.h
+++ b/arm/arm/arm.h
@@ -1,9 +1,9 @@
/*
- * avr_to_arm_uart.h
- *
- * Created: 2019-11-15 14:34:46
- * Author: niljo502
- */
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
+*/
#ifndef ARM_H_
#define ARM_H_
diff --git a/arm/arm/main.c b/arm/arm/main.c
index a448ef3b..6d85dced 100644
--- a/arm/arm/main.c
+++ b/arm/arm/main.c
@@ -1,9 +1,9 @@
/*
- * arm.c
- *
- * Created: 2019-11-15 10:18:36
- * Author: niljo502
- */
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
+*/
//Help-functions for accessing and modifying bytes
#define bit_set(port, bit) (port) |= (1<<(bit))
diff --git a/arm/arm/steering.c b/arm/arm/steering.c
index a9a5bb0b..3f1907bc 100644
--- a/arm/arm/steering.c
+++ b/arm/arm/steering.c
@@ -1,9 +1,9 @@
/*
- * styr.c
- *
- * Created: 2019-11-13 15:29:24
- * Author: niljo502
- */
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
+*/
#define F_CPU 16000000UL
diff --git a/arm/arm/steering.h b/arm/arm/steering.h
index c277c474..521c1f7d 100644
--- a/arm/arm/steering.h
+++ b/arm/arm/steering.h
@@ -1,9 +1,9 @@
/*
- * _steering.h
- *
- * Created: 2019-11-13 15:31:23
- * Author: niljo502
- */
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
+*/
#ifndef _steering_h
#define _steering_h
diff --git a/arm/arm/styr_komm.c b/arm/arm/styr_komm.c
index df06f9f0..f1d1557f 100644
--- a/arm/arm/styr_komm.c
+++ b/arm/arm/styr_komm.c
@@ -1,8 +1,8 @@
/*
-* styr_komm.c
-*
-* Created: 2019-11-21 11:34:02
-* Author: niljo502
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
*/
#include <avr/io.h>
@@ -90,9 +90,9 @@ uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
return arm_rotate_90_CW(arg1,IMODE_WRITE);
case 18: //rotate arm right
return arm_rotate_90_CCW(arg1,IMODE_WRITE);
- case 19: //arm down
+ case 19: //rotate to specified side and extend arm
return extend_arm(arg1);
- case 20://return, rotate and torque
+ case 20://return, rotate and torque Off
return return_arm();
case 21: //arm forward
return arm_forward();
@@ -123,7 +123,7 @@ uint8_t translate_instruction(uint8_t instr, uint8_t arg1, uint8_t arg2){
return increment_servo(arg1, INCREMENT_CONSTANT);
case 57: //Decrement servo
return decrement_servo(arg1, INCREMENT_CONSTANT);
- case 58:
+ case 58: //dropit
return drop_item(arg1);
case 59://Returns both speeds of the wheel pairs. First the left speed then the right.
putchar(SPEED_CONSTANT);
diff --git a/arm/arm/styr_komm.h b/arm/arm/styr_komm.h
index fab5cd88..a63f215d 100644
--- a/arm/arm/styr_komm.h
+++ b/arm/arm/styr_komm.h
@@ -1,9 +1,9 @@
/*
- * styr_komm.h
- *
- * Created: 2019-11-21 11:34:16
- * Author: niljo502
- */
+* Project: RoSS
+* Date: 2019-12-12
+* Version 1.0
+* Author: Jonatan Jonsson: jonjo049, Nils Johansson: niljo502
+*/
#ifndef STYR_KOMM_H_
diff --git a/arm/arm/styrmodul.cproj b/arm/arm/styrmodul.cproj
index f3856712..7a3e95f3 100644
--- a/arm/arm/styrmodul.cproj
+++ b/arm/arm/styrmodul.cproj
@@ -27,16 +27,16 @@
<BootSegment>2</BootSegment>
<eraseonlaunchrule>0</eraseonlaunchrule>
<AsfFrameworkConfig>
- <framework-data xmlns="">
- <options />
- <configurations />
- <files />
- <documentation help="" />
- <offline-documentation help="" />
- <dependencies>
- <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.19.0" />
- </dependencies>
- </framework-data>
+ <framework-data>
+ <options />
+ <configurations />
+ <files />
+ <documentation help="" />
+ <offline-documentation help="" />
+ <dependencies>
+ <content-extension eid="atmel.asf" uuidref="Atmel.ASF" version="3.39.0" />
+ </dependencies>
+</framework-data>
</AsfFrameworkConfig>
<avrtool>com.atmel.avrdbg.tool.atmelice</avrtool>
<com_atmel_avrdbg_tool_atmelice>
@@ -51,85 +51,87 @@
<ToolName>Atmel-ICE</ToolName>
</com_atmel_avrdbg_tool_atmelice>
<avrtoolinterface>JTAG</avrtoolinterface>
+ <ResetRule>0</ResetRule>
+ <EraseKey />
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)' == 'Release' ">
<ToolchainSettings>
<AvrGcc>
- <avrgcc.common.Device>-mmcu=atmega1284p -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\gcc\dev\atmega1284p"</avrgcc.common.Device>
- <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
- <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
- <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
- <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
- <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
- <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
- <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
- <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
- <avrgcc.compiler.symbols.DefSymbols>
- <ListValues>
- <Value>NDEBUG</Value>
- </ListValues>
- </avrgcc.compiler.symbols.DefSymbols>
- <avrgcc.compiler.directories.IncludePaths>
- <ListValues>
- <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
- </ListValues>
- </avrgcc.compiler.directories.IncludePaths>
- <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
- <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
- <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
- <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
- <avrgcc.linker.libraries.Libraries>
- <ListValues>
- <Value>libm</Value>
- </ListValues>
- </avrgcc.linker.libraries.Libraries>
- <avrgcc.assembler.general.IncludePaths>
- <ListValues>
- <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
- </ListValues>
- </avrgcc.assembler.general.IncludePaths>
-</AvrGcc>
+ <avrgcc.common.Device>-mmcu=atmega1284p -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\gcc\dev\atmega1284p"</avrgcc.common.Device>
+ <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
+ <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+ <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+ <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+ <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+ <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+ <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+ <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+ <avrgcc.compiler.symbols.DefSymbols>
+ <ListValues>
+ <Value>NDEBUG</Value>
+ </ListValues>
+ </avrgcc.compiler.symbols.DefSymbols>
+ <avrgcc.compiler.directories.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.compiler.directories.IncludePaths>
+ <avrgcc.compiler.optimization.level>Optimize for size (-Os)</avrgcc.compiler.optimization.level>
+ <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+ <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+ <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+ <avrgcc.linker.libraries.Libraries>
+ <ListValues>
+ <Value>libm</Value>
+ </ListValues>
+ </avrgcc.linker.libraries.Libraries>
+ <avrgcc.assembler.general.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.assembler.general.IncludePaths>
+ </AvrGcc>
</ToolchainSettings>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)' == 'Debug' ">
<ToolchainSettings>
<AvrGcc>
- <avrgcc.common.Device>-mmcu=atmega1284p -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\gcc\dev\atmega1284p"</avrgcc.common.Device>
- <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
- <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
- <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
- <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
- <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
- <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
- <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
- <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
- <avrgcc.compiler.symbols.DefSymbols>
- <ListValues>
- <Value>DEBUG</Value>
- </ListValues>
- </avrgcc.compiler.symbols.DefSymbols>
- <avrgcc.compiler.directories.IncludePaths>
- <ListValues>
- <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
- </ListValues>
- </avrgcc.compiler.directories.IncludePaths>
- <avrgcc.compiler.optimization.level>Optimize (-O1)</avrgcc.compiler.optimization.level>
- <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
- <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
- <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
- <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
- <avrgcc.linker.libraries.Libraries>
- <ListValues>
- <Value>libm</Value>
- </ListValues>
- </avrgcc.linker.libraries.Libraries>
- <avrgcc.assembler.general.IncludePaths>
- <ListValues>
- <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
- </ListValues>
- </avrgcc.assembler.general.IncludePaths>
- <avrgcc.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcc.assembler.debugging.DebugLevel>
-</AvrGcc>
+ <avrgcc.common.Device>-mmcu=atmega1284p -B "%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\gcc\dev\atmega1284p"</avrgcc.common.Device>
+ <avrgcc.common.optimization.RelaxBranches>True</avrgcc.common.optimization.RelaxBranches>
+ <avrgcc.common.outputfiles.hex>True</avrgcc.common.outputfiles.hex>
+ <avrgcc.common.outputfiles.lss>True</avrgcc.common.outputfiles.lss>
+ <avrgcc.common.outputfiles.eep>True</avrgcc.common.outputfiles.eep>
+ <avrgcc.common.outputfiles.srec>True</avrgcc.common.outputfiles.srec>
+ <avrgcc.common.outputfiles.usersignatures>False</avrgcc.common.outputfiles.usersignatures>
+ <avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>True</avrgcc.compiler.general.ChangeDefaultCharTypeUnsigned>
+ <avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>True</avrgcc.compiler.general.ChangeDefaultBitFieldUnsigned>
+ <avrgcc.compiler.symbols.DefSymbols>
+ <ListValues>
+ <Value>DEBUG</Value>
+ </ListValues>
+ </avrgcc.compiler.symbols.DefSymbols>
+ <avrgcc.compiler.directories.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.compiler.directories.IncludePaths>
+ <avrgcc.compiler.optimization.level>Optimize (-O1)</avrgcc.compiler.optimization.level>
+ <avrgcc.compiler.optimization.PackStructureMembers>True</avrgcc.compiler.optimization.PackStructureMembers>
+ <avrgcc.compiler.optimization.AllocateBytesNeededForEnum>True</avrgcc.compiler.optimization.AllocateBytesNeededForEnum>
+ <avrgcc.compiler.optimization.DebugLevel>Default (-g2)</avrgcc.compiler.optimization.DebugLevel>
+ <avrgcc.compiler.warnings.AllWarnings>True</avrgcc.compiler.warnings.AllWarnings>
+ <avrgcc.linker.libraries.Libraries>
+ <ListValues>
+ <Value>libm</Value>
+ </ListValues>
+ </avrgcc.linker.libraries.Libraries>
+ <avrgcc.assembler.general.IncludePaths>
+ <ListValues>
+ <Value>%24(PackRepoDir)\atmel\ATmega_DFP\1.3.300\include</Value>
+ </ListValues>
+ </avrgcc.assembler.general.IncludePaths>
+ <avrgcc.assembler.debugging.DebugLevel>Default (-Wa,-g)</avrgcc.assembler.debugging.DebugLevel>
+ </AvrGcc>
</ToolchainSettings>
</PropertyGroup>
<ItemGroup>
--
GitLab